/*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                        Intel License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2000, Intel Corporation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other materials provided with the distribution.
//
//   * The name of Intel Corporation may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/


#include "precomp.hpp"
#include <float.h>
#include <limits.h>

/* Valery Mosyagin */

#undef quad

#define EPS64D 1e-9

int cvComputeEssentialMatrix(  CvMatr32f rotMatr,
							   CvMatr32f transVect,
							   CvMatr32f essMatr);

int cvConvertEssential2Fundamental( CvMatr32f essMatr,
									CvMatr32f fundMatr,
									CvMatr32f cameraMatr1,
									CvMatr32f cameraMatr2);

int cvComputeEpipolesFromFundMatrix(CvMatr32f fundMatr,
									CvPoint3D32f* epipole1,
									CvPoint3D32f* epipole2);

void icvTestPoint( CvPoint2D64d testPoint,
				   CvVect64d line1, CvVect64d line2,
				   CvPoint2D64d basePoint,
				   int* result);



int icvGetSymPoint3D(  CvPoint3D64d pointCorner,
					   CvPoint3D64d point1,
					   CvPoint3D64d point2,
					   CvPoint3D64d* pointSym2) {
	double len1, len2;
	double alpha;
	icvGetPieceLength3D(pointCorner, point1, &len1);
	if ( len1 < EPS64D ) {
		return CV_BADARG_ERR;
	}
	icvGetPieceLength3D(pointCorner, point2, &len2);
	alpha = len2 / len1;

	pointSym2->x = pointCorner.x + alpha * (point1.x - pointCorner.x);
	pointSym2->y = pointCorner.y + alpha * (point1.y - pointCorner.y);
	pointSym2->z = pointCorner.z + alpha * (point1.z - pointCorner.z);
	return CV_NO_ERR;
}

/*  author Valery Mosyagin */

/* Compute 3D point for scanline and alpha betta */
int icvCompute3DPoint( double alpha, double betta,
					   CvStereoLineCoeff* coeffs,
					   CvPoint3D64d* point) {

	double partX;
	double partY;
	double partZ;
	double partAll;
	double invPartAll;

	double alphabetta = alpha * betta;

	partAll = alpha - betta;
	if ( fabs(partAll) > 0.00001  ) { /* alpha must be > betta */

		partX   = coeffs->Xcoef        + coeffs->XcoefA * alpha +
				  coeffs->XcoefB * betta + coeffs->XcoefAB * alphabetta;

		partY   = coeffs->Ycoef        + coeffs->YcoefA * alpha +
				  coeffs->YcoefB * betta + coeffs->YcoefAB * alphabetta;

		partZ   = coeffs->Zcoef        + coeffs->ZcoefA * alpha +
				  coeffs->ZcoefB * betta + coeffs->ZcoefAB * alphabetta;

		invPartAll = 1.0 / partAll;

		point->x = partX * invPartAll;
		point->y = partY * invPartAll;
		point->z = partZ * invPartAll;
		return CV_NO_ERR;
	} else {
		return CV_BADFACTOR_ERR;
	}
}

/*--------------------------------------------------------------------------------------*/

/* Compute rotate matrix and trans vector for change system */
int icvCreateConvertMatrVect( CvMatr64d     rotMatr1,
							  CvMatr64d     transVect1,
							  CvMatr64d     rotMatr2,
							  CvMatr64d     transVect2,
							  CvMatr64d     convRotMatr,
							  CvMatr64d     convTransVect) {
	double invRotMatr2[9];
	double tmpVect[3];


	icvInvertMatrix_64d(rotMatr2, 3, invRotMatr2);
	/* Test for error */

	icvMulMatrix_64d(   rotMatr1,
						3, 3,
						invRotMatr2,
						3, 3,
						convRotMatr);

	icvMulMatrix_64d(   convRotMatr,
						3, 3,
						transVect2,
						1, 3,
						tmpVect);

	icvSubVector_64d(transVect1, tmpVect, convTransVect, 3);


	return CV_NO_ERR;
}

/*--------------------------------------------------------------------------------------*/

/* Compute point coordinates in other system */
int icvConvertPointSystem(CvPoint3D64d  M2,
						  CvPoint3D64d* M1,
						  CvMatr64d     rotMatr,
						  CvMatr64d     transVect
						 ) {
	double tmpVect[3];

	icvMulMatrix_64d(   rotMatr,
						3, 3,
						(double*)&M2,
						1, 3,
						tmpVect);

	icvAddVector_64d(tmpVect, transVect, (double*)M1, 3);

	return CV_NO_ERR;
}
/*--------------------------------------------------------------------------------------*/
int icvComputeCoeffForStereoV3( double quad1[4][2],
								double quad2[4][2],
								int    numScanlines,
								CvMatr64d    camMatr1,
								CvMatr64d    rotMatr1,
								CvMatr64d    transVect1,
								CvMatr64d    camMatr2,
								CvMatr64d    rotMatr2,
								CvMatr64d    transVect2,
								CvStereoLineCoeff*    startCoeffs,
								int* needSwapCamera) {
	/* For each pair */
	/* In this function we must define position of cameras */

	CvPoint2D64d point1;
	CvPoint2D64d point2;
	CvPoint2D64d point3;
	CvPoint2D64d point4;

	int currLine;
	*needSwapCamera = 0;
	for ( currLine = 0; currLine < numScanlines; currLine++ ) {
		/* Compute points */
		double alpha = ((double)currLine) / ((double)(numScanlines)); /* maybe - 1 */

		point1.x = (1.0 - alpha) * quad1[0][0] + alpha * quad1[3][0];
		point1.y = (1.0 - alpha) * quad1[0][1] + alpha * quad1[3][1];

		point2.x = (1.0 - alpha) * quad1[1][0] + alpha * quad1[2][0];
		point2.y = (1.0 - alpha) * quad1[1][1] + alpha * quad1[2][1];

		point3.x = (1.0 - alpha) * quad2[0][0] + alpha * quad2[3][0];
		point3.y = (1.0 - alpha) * quad2[0][1] + alpha * quad2[3][1];

		point4.x = (1.0 - alpha) * quad2[1][0] + alpha * quad2[2][0];
		point4.y = (1.0 - alpha) * quad2[1][1] + alpha * quad2[2][1];

		/* We can compute coeffs for this line */
		icvComCoeffForLine(    point1,
							   point2,
							   point3,
							   point4,
							   camMatr1,
							   rotMatr1,
							   transVect1,
							   camMatr2,
							   rotMatr2,
							   transVect2,
							   &startCoeffs[currLine],
							   needSwapCamera);
	}
	return CV_NO_ERR;
}
/*--------------------------------------------------------------------------------------*/
int icvComputeCoeffForStereoNew(   double quad1[4][2],
								   double quad2[4][2],
								   int    numScanlines,
								   CvMatr32f    camMatr1,
								   CvMatr32f    rotMatr1,
								   CvMatr32f    transVect1,
								   CvMatr32f    camMatr2,
								   CvStereoLineCoeff*    startCoeffs,
								   int* needSwapCamera) {
	/* Convert data */

	double camMatr1_64d[9];
	double camMatr2_64d[9];

	double rotMatr1_64d[9];
	double transVect1_64d[3];

	double rotMatr2_64d[9];
	double transVect2_64d[3];

	icvCvt_32f_64d(camMatr1, camMatr1_64d, 9);
	icvCvt_32f_64d(camMatr2, camMatr2_64d, 9);

	icvCvt_32f_64d(rotMatr1, rotMatr1_64d, 9);
	icvCvt_32f_64d(transVect1, transVect1_64d, 3);

	rotMatr2_64d[0] = 1;
	rotMatr2_64d[1] = 0;
	rotMatr2_64d[2] = 0;
	rotMatr2_64d[3] = 0;
	rotMatr2_64d[4] = 1;
	rotMatr2_64d[5] = 0;
	rotMatr2_64d[6] = 0;
	rotMatr2_64d[7] = 0;
	rotMatr2_64d[8] = 1;

	transVect2_64d[0] = 0;
	transVect2_64d[1] = 0;
	transVect2_64d[2] = 0;

	int status = icvComputeCoeffForStereoV3( quad1,
				 quad2,
				 numScanlines,
				 camMatr1_64d,
				 rotMatr1_64d,
				 transVect1_64d,
				 camMatr2_64d,
				 rotMatr2_64d,
				 transVect2_64d,
				 startCoeffs,
				 needSwapCamera);


	return status;

}
/*--------------------------------------------------------------------------------------*/
int icvComputeCoeffForStereo(  CvStereoCamera* stereoCamera) {
	double quad1[4][2];
	double quad2[4][2];

	int i;
	for ( i = 0; i < 4; i++ ) {
		quad1[i][0] = stereoCamera->quad[0][i].x;
		quad1[i][1] = stereoCamera->quad[0][i].y;

		quad2[i][0] = stereoCamera->quad[1][i].x;
		quad2[i][1] = stereoCamera->quad[1][i].y;
	}

	icvComputeCoeffForStereoNew(        quad1,
										quad2,
										stereoCamera->warpSize.height,
										stereoCamera->camera[0]->matrix,
										stereoCamera->rotMatrix,
										stereoCamera->transVector,
										stereoCamera->camera[1]->matrix,
										stereoCamera->lineCoeffs,
										&(stereoCamera->needSwapCameras));
	return CV_OK;
}



/*--------------------------------------------------------------------------------------*/
int icvComCoeffForLine(   CvPoint2D64d point1,
						  CvPoint2D64d point2,
						  CvPoint2D64d point3,
						  CvPoint2D64d point4,
						  CvMatr64d    camMatr1,
						  CvMatr64d    rotMatr1,
						  CvMatr64d    transVect1,
						  CvMatr64d    camMatr2,
						  CvMatr64d    rotMatr2,
						  CvMatr64d    transVect2,
						  CvStereoLineCoeff* coeffs,
						  int* needSwapCamera) {
	/* Get direction for all points */
	/* Direction for camera 1 */

	double direct1[3];
	double direct2[3];
	double camPoint1[3];

	double directS3[3];
	double directS4[3];
	double direct3[3];
	double direct4[3];
	double camPoint2[3];

	icvGetDirectionForPoint(   point1,
							   camMatr1,
							   (CvPoint3D64d*)direct1);

	icvGetDirectionForPoint(   point2,
							   camMatr1,
							   (CvPoint3D64d*)direct2);

	/* Direction for camera 2 */

	icvGetDirectionForPoint(   point3,
							   camMatr2,
							   (CvPoint3D64d*)directS3);

	icvGetDirectionForPoint(   point4,
							   camMatr2,
							   (CvPoint3D64d*)directS4);

	/* Create convertion for camera 2: two direction and camera point */

	double convRotMatr[9];
	double convTransVect[3];

	icvCreateConvertMatrVect(  rotMatr1,
							   transVect1,
							   rotMatr2,
							   transVect2,
							   convRotMatr,
							   convTransVect);

	double zeroVect[3];
	zeroVect[0] = zeroVect[1] = zeroVect[2] = 0.0;
	camPoint1[0] = camPoint1[1] = camPoint1[2] = 0.0;

	icvConvertPointSystem(*((CvPoint3D64d*)directS3), (CvPoint3D64d*)direct3, convRotMatr, convTransVect);
	icvConvertPointSystem(*((CvPoint3D64d*)directS4), (CvPoint3D64d*)direct4, convRotMatr, convTransVect);
	icvConvertPointSystem(*((CvPoint3D64d*)zeroVect), (CvPoint3D64d*)camPoint2, convRotMatr, convTransVect);

	double pointB[3];

	int postype = 0;

	/* Changed order */
	/* Compute point B: xB,yB,zB */
	icvGetCrossLines(*((CvPoint3D64d*)camPoint1), *((CvPoint3D64d*)direct2),
					 *((CvPoint3D64d*)camPoint2), *((CvPoint3D64d*)direct3),
					 (CvPoint3D64d*)pointB);

	if ( pointB[2] < 0 ) { /* If negative use other lines for cross */
		postype = 1;
		icvGetCrossLines(*((CvPoint3D64d*)camPoint1), *((CvPoint3D64d*)direct1),
						 *((CvPoint3D64d*)camPoint2), *((CvPoint3D64d*)direct4),
						 (CvPoint3D64d*)pointB);
	}

	CvPoint3D64d pointNewA;
	CvPoint3D64d pointNewC;

	pointNewA.x = pointNewA.y = pointNewA.z = 0;
	pointNewC.x = pointNewC.y = pointNewC.z = 0;

	if ( postype == 0 ) {
		icvGetSymPoint3D(   *((CvPoint3D64d*)camPoint1),
							*((CvPoint3D64d*)direct1),
							*((CvPoint3D64d*)pointB),
							&pointNewA);

		icvGetSymPoint3D(   *((CvPoint3D64d*)camPoint2),
							*((CvPoint3D64d*)direct4),
							*((CvPoint3D64d*)pointB),
							&pointNewC);
	} else {
		/* In this case we must change cameras */
		*needSwapCamera = 1;
		icvGetSymPoint3D(   *((CvPoint3D64d*)camPoint2),
							*((CvPoint3D64d*)direct3),
							*((CvPoint3D64d*)pointB),
							&pointNewA);

		icvGetSymPoint3D(   *((CvPoint3D64d*)camPoint1),
							*((CvPoint3D64d*)direct2),
							*((CvPoint3D64d*)pointB),
							&pointNewC);
	}


	double gamma;

	double x1, y1, z1;

	x1 = camPoint1[0];
	y1 = camPoint1[1];
	z1 = camPoint1[2];

	double xA, yA, zA;
	double xB, yB, zB;
	double xC, yC, zC;

	xA = pointNewA.x;
	yA = pointNewA.y;
	zA = pointNewA.z;

	xB = pointB[0];
	yB = pointB[1];
	zB = pointB[2];

	xC = pointNewC.x;
	yC = pointNewC.y;
	zC = pointNewC.z;

	double len1, len2;
	len1 = sqrt( (xA - xB) * (xA - xB) + (yA - yB) * (yA - yB) + (zA - zB) * (zA - zB) );
	len2 = sqrt( (xB - xC) * (xB - xC) + (yB - yC) * (yB - yC) + (zB - zC) * (zB - zC) );
	gamma = len2 / len1;

	icvComputeStereoLineCoeffs( pointNewA,
								*((CvPoint3D64d*)pointB),
								*((CvPoint3D64d*)camPoint1),
								gamma,
								coeffs);

	return CV_NO_ERR;
}


/*--------------------------------------------------------------------------------------*/

int icvGetDirectionForPoint(  CvPoint2D64d point,
							  CvMatr64d camMatr,
							  CvPoint3D64d* direct) {
	/*  */
	double invMatr[9];

	/* Invert matrix */

	icvInvertMatrix_64d(camMatr, 3, invMatr);
	/* TEST FOR ERRORS */

	double vect[3];
	vect[0] = point.x;
	vect[1] = point.y;
	vect[2] = 1;

	/* Mul matr */
	icvMulMatrix_64d(   invMatr,
						3, 3,
						vect,
						1, 3,
						(double*)direct);

	return CV_NO_ERR;
}

/*--------------------------------------------------------------------------------------*/

int icvGetCrossLines(CvPoint3D64d point11, CvPoint3D64d point12,
					 CvPoint3D64d point21, CvPoint3D64d point22,
					 CvPoint3D64d* midPoint) {
	double xM, yM, zM;
	double xN, yN, zN;

	double xA, yA, zA;
	double xB, yB, zB;

	double xC, yC, zC;
	double xD, yD, zD;

	xA = point11.x;
	yA = point11.y;
	zA = point11.z;

	xB = point12.x;
	yB = point12.y;
	zB = point12.z;

	xC = point21.x;
	yC = point21.y;
	zC = point21.z;

	xD = point22.x;
	yD = point22.y;
	zD = point22.z;

	double a11, a12, a21, a22;
	double b1, b2;

	a11 =  (xB - xA) * (xB - xA) + (yB - yA) * (yB - yA) + (zB - zA) * (zB - zA);
	a12 = -(xD - xC) * (xB - xA) - (yD - yC) * (yB - yA) - (zD - zC) * (zB - zA);
	a21 =  (xB - xA) * (xD - xC) + (yB - yA) * (yD - yC) + (zB - zA) * (zD - zC);
	a22 = -(xD - xC) * (xD - xC) - (yD - yC) * (yD - yC) - (zD - zC) * (zD - zC);
	b1  = -( (xA - xC) * (xB - xA) + (yA - yC) * (yB - yA) + (zA - zC) * (zB - zA) );
	b2  = -( (xA - xC) * (xD - xC) + (yA - yC) * (yD - yC) + (zA - zC) * (zD - zC) );

	double delta;
	double deltaA, deltaB;
	double alpha, betta;

	delta  = a11 * a22 - a12 * a21;

	if ( fabs(delta) < EPS64D ) {
		/*return ERROR;*/
	}

	deltaA = b1 * a22 - b2 * a12;
	deltaB = a11 * b2 - b1 * a21;

	alpha = deltaA / delta;
	betta = deltaB / delta;

	xM = xA + alpha * (xB - xA);
	yM = yA + alpha * (yB - yA);
	zM = zA + alpha * (zB - zA);

	xN = xC + betta * (xD - xC);
	yN = yC + betta * (yD - yC);
	zN = zC + betta * (zD - zC);

	/* Compute middle point */
	midPoint->x = (xM + xN) * 0.5;
	midPoint->y = (yM + yN) * 0.5;
	midPoint->z = (zM + zN) * 0.5;

	return CV_NO_ERR;
}

/*--------------------------------------------------------------------------------------*/

int icvComputeStereoLineCoeffs(   CvPoint3D64d pointA,
								  CvPoint3D64d pointB,
								  CvPoint3D64d pointCam1,
								  double gamma,
								  CvStereoLineCoeff*    coeffs) {
	double x1, y1, z1;

	x1 = pointCam1.x;
	y1 = pointCam1.y;
	z1 = pointCam1.z;

	double xA, yA, zA;
	double xB, yB, zB;

	xA = pointA.x;
	yA = pointA.y;
	zA = pointA.z;

	xB = pointB.x;
	yB = pointB.y;
	zB = pointB.z;

	if ( gamma > 0 ) {
		coeffs->Xcoef   = -x1 + xA;
		coeffs->XcoefA  =  xB + x1 - xA;
		coeffs->XcoefB  = -xA - gamma * x1 + gamma * xA;
		coeffs->XcoefAB = -xB + xA + gamma * xB - gamma * xA;

		coeffs->Ycoef   = -y1 + yA;
		coeffs->YcoefA  =  yB + y1 - yA;
		coeffs->YcoefB  = -yA - gamma * y1 + gamma * yA;
		coeffs->YcoefAB = -yB + yA + gamma * yB - gamma * yA;

		coeffs->Zcoef   = -z1 + zA;
		coeffs->ZcoefA  =  zB + z1 - zA;
		coeffs->ZcoefB  = -zA - gamma * z1 + gamma * zA;
		coeffs->ZcoefAB = -zB + zA + gamma * zB - gamma * zA;
	} else {
		gamma = - gamma;
		coeffs->Xcoef   = -( -x1 + xA);
		coeffs->XcoefB  = -(  xB + x1 - xA);
		coeffs->XcoefA  = -( -xA - gamma * x1 + gamma * xA);
		coeffs->XcoefAB = -( -xB + xA + gamma * xB - gamma * xA);

		coeffs->Ycoef   = -( -y1 + yA);
		coeffs->YcoefB  = -(  yB + y1 - yA);
		coeffs->YcoefA  = -( -yA - gamma * y1 + gamma * yA);
		coeffs->YcoefAB = -( -yB + yA + gamma * yB - gamma * yA);

		coeffs->Zcoef   = -( -z1 + zA);
		coeffs->ZcoefB  = -(  zB + z1 - zA);
		coeffs->ZcoefA  = -( -zA - gamma * z1 + gamma * zA);
		coeffs->ZcoefAB = -( -zB + zA + gamma * zB - gamma * zA);
	}



	return CV_NO_ERR;
}
/*--------------------------------------------------------------------------------------*/


/*---------------------------------------------------------------------------------------*/

/* This function get minimum angle started at point which contains rect */
int icvGetAngleLine( CvPoint2D64d startPoint, CvSize imageSize, CvPoint2D64d* point1, CvPoint2D64d* point2) {
	/* Get crosslines with image corners */

	/* Find four lines */

	CvPoint2D64d pa, pb, pc, pd;

	pa.x = 0;
	pa.y = 0;

	pb.x = imageSize.width - 1;
	pb.y = 0;

	pd.x = imageSize.width - 1;
	pd.y = imageSize.height - 1;

	pc.x = 0;
	pc.y = imageSize.height - 1;

	/* We can compute points for angle */
	/* Test for place section */

	if ( startPoint.x < 0 ) {
		/* 1,4,7 */
		if ( startPoint.y < 0) {
			/* 1 */
			*point1 = pb;
			*point2 = pc;
		} else if ( startPoint.y > imageSize.height - 1 ) {
			/* 7 */
			*point1 = pa;
			*point2 = pd;
		} else {
			/* 4 */
			*point1 = pa;
			*point2 = pc;
		}
	} else if ( startPoint.x > imageSize.width - 1 ) {
		/* 3,6,9 */
		if ( startPoint.y < 0 ) {
			/* 3 */
			*point1 = pa;
			*point2 = pd;
		} else if ( startPoint.y > imageSize.height - 1 ) {
			/* 9 */
			*point1 = pb;
			*point2 = pc;
		} else {
			/* 6 */
			*point1 = pb;
			*point2 = pd;
		}
	} else {
		/* 2,5,8 */
		if ( startPoint.y < 0 ) {
			/* 2 */
			if ( startPoint.x < imageSize.width / 2 ) {
				*point1 = pb;
				*point2 = pa;
			} else {
				*point1 = pa;
				*point2 = pb;
			}
		} else if ( startPoint.y > imageSize.height - 1 ) {
			/* 8 */
			if ( startPoint.x < imageSize.width / 2 ) {
				*point1 = pc;
				*point2 = pd;
			} else {
				*point1 = pd;
				*point2 = pc;
			}
		} else {
			/* 5 - point in the image */
			return 2;
		}
	}
	return 0;
}/* GetAngleLine */

/*---------------------------------------------------------------------------------------*/

void icvGetCoefForPiece(   CvPoint2D64d p_start, CvPoint2D64d p_end,
						   double* a, double* b, double* c,
						   int* result) {
	double det;
	double detA, detB, detC;

	det = p_start.x * p_end.y + p_end.x + p_start.y - p_end.y - p_start.y * p_end.x - p_start.x;
	if ( fabs(det) < EPS64D) { /* Error */
		*result = 0;
		return;
	}

	detA = p_start.y - p_end.y;
	detB = p_end.x - p_start.x;
	detC = p_start.x * p_end.y - p_end.x * p_start.y;

	double invDet = 1.0 / det;
	*a = detA * invDet;
	*b = detB * invDet;
	*c = detC * invDet;

	*result = 1;
	return;
}

/*---------------------------------------------------------------------------------------*/

/* Get common area of rectifying */
void icvGetCommonArea( CvSize imageSize,
					   CvPoint3D64d epipole1, CvPoint3D64d epipole2,
					   CvMatr64d fundMatr,
					   CvVect64d coeff11, CvVect64d coeff12,
					   CvVect64d coeff21, CvVect64d coeff22,
					   int* result) {
	int res = 0;
	CvPoint2D64d point11;
	CvPoint2D64d point12;
	CvPoint2D64d point21;
	CvPoint2D64d point22;

	double corr11[3];
	double corr12[3];
	double corr21[3];
	double corr22[3];

	double pointW11[3];
	double pointW12[3];
	double pointW21[3];
	double pointW22[3];

	double transFundMatr[3*3];
	/* Compute transpose of fundamental matrix */
	icvTransposeMatrix_64d( fundMatr, 3, 3, transFundMatr );

	CvPoint2D64d epipole1_2d;
	CvPoint2D64d epipole2_2d;

	if ( fabs(epipole1.z) < 1e-8 ) {
		/* epipole1 in infinity */
		*result = 0;
		return;
	}
	epipole1_2d.x = epipole1.x / epipole1.z;
	epipole1_2d.y = epipole1.y / epipole1.z;

	if ( fabs(epipole2.z) < 1e-8 ) {
		/* epipole2 in infinity */
		*result = 0;
		return;
	}
	epipole2_2d.x = epipole2.x / epipole2.z;
	epipole2_2d.y = epipole2.y / epipole2.z;

	int stat = icvGetAngleLine( epipole1_2d, imageSize, &point11, &point12);
	if ( stat == 2 ) {
		/* No angle */
		*result = 0;
		return;
	}

	stat = icvGetAngleLine( epipole2_2d, imageSize, &point21, &point22);
	if ( stat == 2 ) {
		/* No angle */
		*result = 0;
		return;
	}

	/* ============= Computation for line 1 ================ */
	/* Find correspondence line for angle points11 */
	/* corr21 = Fund'*p1 */

	pointW11[0] = point11.x;
	pointW11[1] = point11.y;
	pointW11[2] = 1.0;

	icvTransformVector_64d( transFundMatr, /* !!! Modified from not transposed */
							pointW11,
							corr21,
							3, 3);

	/* Find crossing of line with image 2 */
	CvPoint2D64d start;
	CvPoint2D64d end;
	icvGetCrossRectDirect( imageSize,
						   corr21[0], corr21[1], corr21[2],
						   &start, &end,
						   &res);

	if ( res == 0 ) {
		/* We have not cross */
		/* We must define new angle */

		pointW21[0] = point21.x;
		pointW21[1] = point21.y;
		pointW21[2] = 1.0;

		/* Find correspondence line for this angle points */
		/* We know point and try to get corr line */
		/* For point21 */
		/* corr11 = Fund * p21 */

		icvTransformVector_64d( fundMatr, /* !!! Modified */
								pointW21,
								corr11,
								3, 3);

		/* We have cross. And it's result cross for up line. Set result coefs */

		/* Set coefs for line 1 image 1 */
		coeff11[0] = corr11[0];
		coeff11[1] = corr11[1];
		coeff11[2] = corr11[2];

		/* Set coefs for line 1 image 2 */
		icvGetCoefForPiece(    epipole2_2d, point21,
							   &coeff21[0], &coeff21[1], &coeff21[2],
							   &res);
		if ( res == 0 ) {
			*result = 0;
			return;/* Error */
		}
	} else {
		/* Line 1 cross image 2 */
		/* Set coefs for line 1 image 1 */
		icvGetCoefForPiece(    epipole1_2d, point11,
							   &coeff11[0], &coeff11[1], &coeff11[2],
							   &res);
		if ( res == 0 ) {
			*result = 0;
			return;/* Error */
		}

		/* Set coefs for line 1 image 2 */
		coeff21[0] = corr21[0];
		coeff21[1] = corr21[1];
		coeff21[2] = corr21[2];

	}

	/* ============= Computation for line 2 ================ */
	/* Find correspondence line for angle points11 */
	/* corr22 = Fund*p2 */

	pointW12[0] = point12.x;
	pointW12[1] = point12.y;
	pointW12[2] = 1.0;

	icvTransformVector_64d( transFundMatr,
							pointW12,
							corr22,
							3, 3);

	/* Find crossing of line with image 2 */
	icvGetCrossRectDirect( imageSize,
						   corr22[0], corr22[1], corr22[2],
						   &start, &end,
						   &res);

	if ( res == 0 ) {
		/* We have not cross */
		/* We must define new angle */

		pointW22[0] = point22.x;
		pointW22[1] = point22.y;
		pointW22[2] = 1.0;

		/* Find correspondence line for this angle points */
		/* We know point and try to get corr line */
		/* For point21 */
		/* corr2 = Fund' * p1 */

		icvTransformVector_64d( fundMatr,
								pointW22,
								corr12,
								3, 3);


		/* We have cross. And it's result cross for down line. Set result coefs */

		/* Set coefs for line 2 image 1 */
		coeff12[0] = corr12[0];
		coeff12[1] = corr12[1];
		coeff12[2] = corr12[2];

		/* Set coefs for line 1 image 2 */
		icvGetCoefForPiece(    epipole2_2d, point22,
							   &coeff22[0], &coeff22[1], &coeff22[2],
							   &res);
		if ( res == 0 ) {
			*result = 0;
			return;/* Error */
		}
	} else {
		/* Line 2 cross image 2 */
		/* Set coefs for line 2 image 1 */
		icvGetCoefForPiece(    epipole1_2d, point12,
							   &coeff12[0], &coeff12[1], &coeff12[2],
							   &res);
		if ( res == 0 ) {
			*result = 0;
			return;/* Error */
		}

		/* Set coefs for line 1 image 2 */
		coeff22[0] = corr22[0];
		coeff22[1] = corr22[1];
		coeff22[2] = corr22[2];

	}

	/* Now we know common area */

	return;

}/* GetCommonArea */

/*---------------------------------------------------------------------------------------*/

/* Get cross for direction1 and direction2 */
/*  Result = 1 - cross */
/*  Result = 2 - parallel and not equal */
/*  Result = 3 - parallel and equal */

void icvGetCrossDirectDirect(  CvVect64d direct1, CvVect64d direct2,
							   CvPoint2D64d* cross, int* result) {
	double det  = direct1[0] * direct2[1] - direct2[0] * direct1[1];
	double detx = -direct1[2] * direct2[1] + direct1[1] * direct2[2];

	if ( fabs(det) > EPS64D ) {
		/* Have cross */
		cross->x = detx / det;
		cross->y = (-direct1[0] * direct2[2] + direct2[0] * direct1[2]) / det;
		*result = 1;
	} else {
		/* may be parallel */
		if ( fabs(detx) > EPS64D ) {
			/* parallel and not equal */
			*result = 2;
		} else {
			/* equals */
			*result = 3;
		}
	}

	return;
}

/*---------------------------------------------------------------------------------------*/

/* Get cross for piece p1,p2 and direction a,b,c */
/*  Result = 0 - no cross */
/*  Result = 1 - cross */
/*  Result = 2 - parallel and not equal */
/*  Result = 3 - parallel and equal */

void icvGetCrossPieceDirect(   CvPoint2D64d p_start, CvPoint2D64d p_end,
							   double a, double b, double c,
							   CvPoint2D64d* cross, int* result) {

	if ( (a * p_start.x + b * p_start.y + c) * (a * p_end.x + b * p_end.y + c) <= 0 ) {
		/* Have cross */
		double det;
		double detxc, detyc;

		det = a * (p_end.x - p_start.x) + b * (p_end.y - p_start.y);

		if ( fabs(det) < EPS64D ) {
			/* lines are parallel and may be equal or line is point */
			if (  fabs(a * p_start.x + b * p_start.y + c) < EPS64D ) {
				/* line is point or not diff */
				*result = 3;
				return;
			} else {
				*result = 2;
			}
			return;
		}

		detxc = b * (p_end.y * p_start.x - p_start.y * p_end.x) + c * (p_start.x - p_end.x);
		detyc = a * (p_end.x * p_start.y - p_start.x * p_end.y) + c * (p_start.y - p_end.y);

		cross->x = detxc / det;
		cross->y = detyc / det;
		*result = 1;

	} else {
		*result = 0;
	}
	return;
}
/*--------------------------------------------------------------------------------------*/

void icvGetCrossPiecePiece( CvPoint2D64d p1_start, CvPoint2D64d p1_end,
							CvPoint2D64d p2_start, CvPoint2D64d p2_end,
							CvPoint2D64d* cross,
							int* result) {
	double ex1, ey1, ex2, ey2;
	double px1, py1, px2, py2;
	double del;
	double delA, delB, delX, delY;
	double alpha, betta;

	ex1 = p1_start.x;
	ey1 = p1_start.y;
	ex2 = p1_end.x;
	ey2 = p1_end.y;

	px1 = p2_start.x;
	py1 = p2_start.y;
	px2 = p2_end.x;
	py2 = p2_end.y;

	del = (py1 - py2) * (ex1 - ex2) - (px1 - px2) * (ey1 - ey2);
	if ( fabs(del) <= EPS64D ) {
		/* May be they are parallel !!! */
		*result = 0;
		return;
	}

	delA =  (ey1 - ey2) * (ex1 - px1) + (ex1 - ex2) * (py1 - ey1);
	delB =  (py1 - py2) * (ex1 - px1) + (px1 - px2) * (py1 - ey1);

	alpha = delA / del;
	betta = delB / del;

	if ( alpha < 0 || alpha > 1.0 || betta < 0 || betta > 1.0) {
		*result = 0;
		return;
	}

	delX =  (px1 - px2) * (ey1 * (ex1 - ex2) - ex1 * (ey1 - ey2)) +
			(ex1 - ex2) * (px1 * (py1 - py2) - py1 * (px1 - px2));

	delY =  (py1 - py2) * (ey1 * (ex1 - ex2) - ex1 * (ey1 - ey2)) +
			(ey1 - ey2) * (px1 * (py1 - py2) - py1 * (px1 - px2));

	cross->x = delX / del;
	cross->y = delY / del;

	*result = 1;
	return;
}


/*---------------------------------------------------------------------------------------*/

void icvGetPieceLength(CvPoint2D64d point1, CvPoint2D64d point2, double* dist) {
	double dx = point2.x - point1.x;
	double dy = point2.y - point1.y;
	*dist = sqrt( dx * dx + dy * dy );
	return;
}

/*---------------------------------------------------------------------------------------*/

void icvGetPieceLength3D(CvPoint3D64d point1, CvPoint3D64d point2, double* dist) {
	double dx = point2.x - point1.x;
	double dy = point2.y - point1.y;
	double dz = point2.z - point1.z;
	*dist = sqrt( dx * dx + dy * dy + dz * dz );
	return;
}

/*---------------------------------------------------------------------------------------*/

/* Find line from epipole which cross image rect */
/* Find points of cross 0 or 1 or 2. Return number of points in cross */
void icvGetCrossRectDirect(    CvSize imageSize,
							   double a, double b, double c,
							   CvPoint2D64d* start, CvPoint2D64d* end,
							   int* result) {
	CvPoint2D64d frameBeg;
	CvPoint2D64d frameEnd;
	CvPoint2D64d cross[4];
	int     haveCross[4];

	haveCross[0] = 0;
	haveCross[1] = 0;
	haveCross[2] = 0;
	haveCross[3] = 0;

	frameBeg.x = 0;
	frameBeg.y = 0;
	frameEnd.x = imageSize.width;
	frameEnd.y = 0;

	icvGetCrossPieceDirect(frameBeg, frameEnd, a, b, c, &cross[0], &haveCross[0]);

	frameBeg.x = imageSize.width;
	frameBeg.y = 0;
	frameEnd.x = imageSize.width;
	frameEnd.y = imageSize.height;
	icvGetCrossPieceDirect(frameBeg, frameEnd, a, b, c, &cross[1], &haveCross[1]);

	frameBeg.x = imageSize.width;
	frameBeg.y = imageSize.height;
	frameEnd.x = 0;
	frameEnd.y = imageSize.height;
	icvGetCrossPieceDirect(frameBeg, frameEnd, a, b, c, &cross[2], &haveCross[2]);

	frameBeg.x = 0;
	frameBeg.y = imageSize.height;
	frameEnd.x = 0;
	frameEnd.y = 0;
	icvGetCrossPieceDirect(frameBeg, frameEnd, a, b, c, &cross[3], &haveCross[3]);

	double maxDist;

	int maxI = 0, maxJ = 0;


	int i, j;

	maxDist = -1.0;

	double distance;

	for ( i = 0; i < 3; i++ ) {
		if ( haveCross[i] == 1 ) {
			for ( j = i + 1; j < 4; j++ ) {
				if ( haveCross[j] == 1) {
					/* Compute dist */
					icvGetPieceLength(cross[i], cross[j], &distance);
					if ( distance > maxDist ) {
						maxI = i;
						maxJ = j;
						maxDist = distance;
					}
				}
			}
		}
	}

	if ( maxDist >= 0 ) {
		/* We have cross */
		*start = cross[maxI];
		*result = 1;
		if ( maxDist > 0 ) {
			*end   = cross[maxJ];
			*result = 2;
		}
	} else {
		*result = 0;
	}

	return;
}/* GetCrossRectDirect */

/*---------------------------------------------------------------------------------------*/
void icvProjectPointToImage(   CvPoint3D64d point,
							   CvMatr64d camMatr, CvMatr64d rotMatr, CvVect64d transVect,
							   CvPoint2D64d* projPoint) {

	double tmpVect1[3];
	double tmpVect2[3];

	icvMulMatrix_64d (  rotMatr,
						3, 3,
						(double*)&point,
						1, 3,
						tmpVect1);

	icvAddVector_64d ( tmpVect1, transVect, tmpVect2, 3);

	icvMulMatrix_64d (  camMatr,
						3, 3,
						tmpVect2,
						1, 3,
						tmpVect1);

	projPoint->x = tmpVect1[0] / tmpVect1[2];
	projPoint->y = tmpVect1[1] / tmpVect1[2];

	return;
}

/*---------------------------------------------------------------------------------------*/
/* Get quads for transform images */
void icvGetQuadsTransform(
	CvSize        imageSize,
	CvMatr64d     camMatr1,
	CvMatr64d     rotMatr1,
	CvVect64d     transVect1,
	CvMatr64d     camMatr2,
	CvMatr64d     rotMatr2,
	CvVect64d     transVect2,
	CvSize*       warpSize,
	double quad1[4][2],
	double quad2[4][2],
	CvMatr64d     fundMatr,
	CvPoint3D64d* epipole1,
	CvPoint3D64d* epipole2
) {
	/* First compute fundamental matrix and epipoles */
	int res;


	/* Compute epipoles and fundamental matrix using new functions */
	{
		double convRotMatr[9];
		double convTransVect[3];

		icvCreateConvertMatrVect( rotMatr1,
								  transVect1,
								  rotMatr2,
								  transVect2,
								  convRotMatr,
								  convTransVect);
		float convRotMatr_32f[9];
		float convTransVect_32f[3];

		icvCvt_64d_32f(convRotMatr, convRotMatr_32f, 9);
		icvCvt_64d_32f(convTransVect, convTransVect_32f, 3);

		/* We know R and t */
		/* Compute essential matrix */
		float essMatr[9];
		float fundMatr_32f[9];

		float camMatr1_32f[9];
		float camMatr2_32f[9];

		icvCvt_64d_32f(camMatr1, camMatr1_32f, 9);
		icvCvt_64d_32f(camMatr2, camMatr2_32f, 9);

		cvComputeEssentialMatrix(   convRotMatr_32f,
									convTransVect_32f,
									essMatr);

		cvConvertEssential2Fundamental( essMatr,
										fundMatr_32f,
										camMatr1_32f,
										camMatr2_32f);

		CvPoint3D32f epipole1_32f;
		CvPoint3D32f epipole2_32f;

		cvComputeEpipolesFromFundMatrix( fundMatr_32f,
										 &epipole1_32f,
										 &epipole2_32f);
		/* copy to 64d epipoles */
		epipole1->x = epipole1_32f.x;
		epipole1->y = epipole1_32f.y;
		epipole1->z = epipole1_32f.z;

		epipole2->x = epipole2_32f.x;
		epipole2->y = epipole2_32f.y;
		epipole2->z = epipole2_32f.z;

		/* Convert fundamental matrix */
		icvCvt_32f_64d(fundMatr_32f, fundMatr, 9);
	}

	double coeff11[3];
	double coeff12[3];
	double coeff21[3];
	double coeff22[3];

	icvGetCommonArea(   imageSize,
						*epipole1, *epipole2,
						fundMatr,
						coeff11, coeff12,
						coeff21, coeff22,
						&res);

	CvPoint2D64d point11, point12, point21, point22;
	double width1, width2;
	double height1, height2;
	double tmpHeight1, tmpHeight2;

	CvPoint2D64d epipole1_2d;
	CvPoint2D64d epipole2_2d;

	/* ----- Image 1 ----- */
	if ( fabs(epipole1->z) < 1e-8 ) {
		return;
	}
	epipole1_2d.x = epipole1->x / epipole1->z;
	epipole1_2d.y = epipole1->y / epipole1->z;

	icvGetCutPiece( coeff11, coeff12,
					epipole1_2d,
					imageSize,
					&point11, &point12,
					&point21, &point22,
					&res);

	/* Compute distance */
	icvGetPieceLength(point11, point21, &width1);
	icvGetPieceLength(point11, point12, &tmpHeight1);
	icvGetPieceLength(point21, point22, &tmpHeight2);
	height1 = MAX(tmpHeight1, tmpHeight2);

	quad1[0][0] = point11.x;
	quad1[0][1] = point11.y;

	quad1[1][0] = point21.x;
	quad1[1][1] = point21.y;

	quad1[2][0] = point22.x;
	quad1[2][1] = point22.y;

	quad1[3][0] = point12.x;
	quad1[3][1] = point12.y;

	/* ----- Image 2 ----- */
	if ( fabs(epipole2->z) < 1e-8 ) {
		return;
	}
	epipole2_2d.x = epipole2->x / epipole2->z;
	epipole2_2d.y = epipole2->y / epipole2->z;

	icvGetCutPiece( coeff21, coeff22,
					epipole2_2d,
					imageSize,
					&point11, &point12,
					&point21, &point22,
					&res);

	/* Compute distance */
	icvGetPieceLength(point11, point21, &width2);
	icvGetPieceLength(point11, point12, &tmpHeight1);
	icvGetPieceLength(point21, point22, &tmpHeight2);
	height2 = MAX(tmpHeight1, tmpHeight2);

	quad2[0][0] = point11.x;
	quad2[0][1] = point11.y;

	quad2[1][0] = point21.x;
	quad2[1][1] = point21.y;

	quad2[2][0] = point22.x;
	quad2[2][1] = point22.y;

	quad2[3][0] = point12.x;
	quad2[3][1] = point12.y;


	/*=======================================================*/
	/* This is a new additional way to compute quads. */
	/* We must correct quads */
	{
		double convRotMatr[9];
		double convTransVect[3];

		double newQuad1[4][2];
		double newQuad2[4][2];


		icvCreateConvertMatrVect( rotMatr1,
								  transVect1,
								  rotMatr2,
								  transVect2,
								  convRotMatr,
								  convTransVect);

		/* -------------Compute for first image-------------- */
		CvPoint2D32f pointb1;
		CvPoint2D32f pointe1;

		CvPoint2D32f pointb2;
		CvPoint2D32f pointe2;

		pointb1.x = (float)quad1[0][0];
		pointb1.y = (float)quad1[0][1];

		pointe1.x = (float)quad1[3][0];
		pointe1.y = (float)quad1[3][1];

		icvComputeeInfiniteProject1(convRotMatr,
									camMatr1,
									camMatr2,
									pointb1,
									&pointb2);

		icvComputeeInfiniteProject1(convRotMatr,
									camMatr1,
									camMatr2,
									pointe1,
									&pointe2);

		/*  JUST TEST FOR POINT */

		/* Compute distances */
		double dxOld, dyOld;
		double dxNew, dyNew;
		double distOld, distNew;

		dxOld = quad2[1][0] - quad2[0][0];
		dyOld = quad2[1][1] - quad2[0][1];
		distOld = dxOld * dxOld + dyOld * dyOld;

		dxNew = quad2[1][0] - pointb2.x;
		dyNew = quad2[1][1] - pointb2.y;
		distNew = dxNew * dxNew + dyNew * dyNew;

		if ( distNew > distOld ) {
			/* Get new points for second quad */
			newQuad2[0][0] = pointb2.x;
			newQuad2[0][1] = pointb2.y;
			newQuad2[3][0] = pointe2.x;
			newQuad2[3][1] = pointe2.y;
			newQuad1[0][0] = quad1[0][0];
			newQuad1[0][1] = quad1[0][1];
			newQuad1[3][0] = quad1[3][0];
			newQuad1[3][1] = quad1[3][1];
		} else {
			/* Get new points for first quad */

			pointb2.x = (float)quad2[0][0];
			pointb2.y = (float)quad2[0][1];

			pointe2.x = (float)quad2[3][0];
			pointe2.y = (float)quad2[3][1];

			icvComputeeInfiniteProject2(convRotMatr,
										camMatr1,
										camMatr2,
										&pointb1,
										pointb2);

			icvComputeeInfiniteProject2(convRotMatr,
										camMatr1,
										camMatr2,
										&pointe1,
										pointe2);


			/*  JUST TEST FOR POINT */

			newQuad2[0][0] = quad2[0][0];
			newQuad2[0][1] = quad2[0][1];
			newQuad2[3][0] = quad2[3][0];
			newQuad2[3][1] = quad2[3][1];

			newQuad1[0][0] = pointb1.x;
			newQuad1[0][1] = pointb1.y;
			newQuad1[3][0] = pointe1.x;
			newQuad1[3][1] = pointe1.y;
		}

		/* -------------Compute for second image-------------- */
		pointb1.x = (float)quad1[1][0];
		pointb1.y = (float)quad1[1][1];

		pointe1.x = (float)quad1[2][0];
		pointe1.y = (float)quad1[2][1];

		icvComputeeInfiniteProject1(convRotMatr,
									camMatr1,
									camMatr2,
									pointb1,
									&pointb2);

		icvComputeeInfiniteProject1(convRotMatr,
									camMatr1,
									camMatr2,
									pointe1,
									&pointe2);

		/* Compute distances */

		dxOld = quad2[0][0] - quad2[1][0];
		dyOld = quad2[0][1] - quad2[1][1];
		distOld = dxOld * dxOld + dyOld * dyOld;

		dxNew = quad2[0][0] - pointb2.x;
		dyNew = quad2[0][1] - pointb2.y;
		distNew = dxNew * dxNew + dyNew * dyNew;

		if ( distNew > distOld ) {
			/* Get new points for second quad */
			newQuad2[1][0] = pointb2.x;
			newQuad2[1][1] = pointb2.y;
			newQuad2[2][0] = pointe2.x;
			newQuad2[2][1] = pointe2.y;
			newQuad1[1][0] = quad1[1][0];
			newQuad1[1][1] = quad1[1][1];
			newQuad1[2][0] = quad1[2][0];
			newQuad1[2][1] = quad1[2][1];
		} else {
			/* Get new points for first quad */

			pointb2.x = (float)quad2[1][0];
			pointb2.y = (float)quad2[1][1];

			pointe2.x = (float)quad2[2][0];
			pointe2.y = (float)quad2[2][1];

			icvComputeeInfiniteProject2(convRotMatr,
										camMatr1,
										camMatr2,
										&pointb1,
										pointb2);

			icvComputeeInfiniteProject2(convRotMatr,
										camMatr1,
										camMatr2,
										&pointe1,
										pointe2);

			newQuad2[1][0] = quad2[1][0];
			newQuad2[1][1] = quad2[1][1];
			newQuad2[2][0] = quad2[2][0];
			newQuad2[2][1] = quad2[2][1];

			newQuad1[1][0] = pointb1.x;
			newQuad1[1][1] = pointb1.y;
			newQuad1[2][0] = pointe1.x;
			newQuad1[2][1] = pointe1.y;
		}



		/*-------------------------------------------------------------------------------*/

		/* Copy new quads to old quad */
		int i;
		for ( i = 0; i < 4; i++ ) {
			{
				quad1[i][0] = newQuad1[i][0];
				quad1[i][1] = newQuad1[i][1];
				quad2[i][0] = newQuad2[i][0];
				quad2[i][1] = newQuad2[i][1];
			}
		}
	}
	/*=======================================================*/

	double warpWidth, warpHeight;

	warpWidth  = MAX(width1, width2);
	warpHeight = MAX(height1, height2);

	warpSize->width  = (int)warpWidth;
	warpSize->height = (int)warpHeight;

	warpSize->width  = cvRound(warpWidth - 1);
	warpSize->height = cvRound(warpHeight - 1);

	/* !!! by Valery Mosyagin. this lines added just for test no warp */
	warpSize->width  = imageSize.width;
	warpSize->height = imageSize.height;

	return;
}


/*---------------------------------------------------------------------------------------*/

void icvGetQuadsTransformNew(  CvSize        imageSize,
							   CvMatr32f     camMatr1,
							   CvMatr32f     camMatr2,
							   CvMatr32f     rotMatr1,
							   CvVect32f     transVect1,
							   CvSize*       warpSize,
							   double        quad1[4][2],
							   double        quad2[4][2],
							   CvMatr32f     fundMatr,
							   CvPoint3D32f* epipole1,
							   CvPoint3D32f* epipole2
							) {
	/* Convert data */
	/* Convert camera matrix */
	double camMatr1_64d[9];
	double camMatr2_64d[9];
	double rotMatr1_64d[9];
	double transVect1_64d[3];
	double rotMatr2_64d[9];
	double transVect2_64d[3];
	double fundMatr_64d[9];
	CvPoint3D64d epipole1_64d;
	CvPoint3D64d epipole2_64d;

	icvCvt_32f_64d(camMatr1, camMatr1_64d, 9);
	icvCvt_32f_64d(camMatr2, camMatr2_64d, 9);
	icvCvt_32f_64d(rotMatr1, rotMatr1_64d, 9);
	icvCvt_32f_64d(transVect1, transVect1_64d, 3);

	/* Create vector and matrix */

	rotMatr2_64d[0] = 1;
	rotMatr2_64d[1] = 0;
	rotMatr2_64d[2] = 0;
	rotMatr2_64d[3] = 0;
	rotMatr2_64d[4] = 1;
	rotMatr2_64d[5] = 0;
	rotMatr2_64d[6] = 0;
	rotMatr2_64d[7] = 0;
	rotMatr2_64d[8] = 1;

	transVect2_64d[0] = 0;
	transVect2_64d[1] = 0;
	transVect2_64d[2] = 0;

	icvGetQuadsTransform(   imageSize,
							camMatr1_64d,
							rotMatr1_64d,
							transVect1_64d,
							camMatr2_64d,
							rotMatr2_64d,
							transVect2_64d,
							warpSize,
							quad1,
							quad2,
							fundMatr_64d,
							&epipole1_64d,
							&epipole2_64d
						);

	/* Convert epipoles */
	epipole1->x = (float)(epipole1_64d.x);
	epipole1->y = (float)(epipole1_64d.y);
	epipole1->z = (float)(epipole1_64d.z);

	epipole2->x = (float)(epipole2_64d.x);
	epipole2->y = (float)(epipole2_64d.y);
	epipole2->z = (float)(epipole2_64d.z);

	/* Convert fundamental matrix */
	icvCvt_64d_32f(fundMatr_64d, fundMatr, 9);

	return;
}

/*---------------------------------------------------------------------------------------*/
void icvGetQuadsTransformStruct(  CvStereoCamera* stereoCamera) {
	/* Wrapper for icvGetQuadsTransformNew */


	double  quad1[4][2];
	double  quad2[4][2];

	icvGetQuadsTransformNew(     cvSize(cvRound(stereoCamera->camera[0]->imgSize[0]), cvRound(stereoCamera->camera[0]->imgSize[1])),
								 stereoCamera->camera[0]->matrix,
								 stereoCamera->camera[1]->matrix,
								 stereoCamera->rotMatrix,
								 stereoCamera->transVector,
								 &(stereoCamera->warpSize),
								 quad1,
								 quad2,
								 stereoCamera->fundMatr,
								 &(stereoCamera->epipole[0]),
								 &(stereoCamera->epipole[1])
						   );

	int i;
	for ( i = 0; i < 4; i++ ) {
		stereoCamera->quad[0][i] = cvPoint2D32f(quad1[i][0], quad1[i][1]);
		stereoCamera->quad[1][i] = cvPoint2D32f(quad2[i][0], quad2[i][1]);
	}

	return;
}

/*---------------------------------------------------------------------------------------*/
void icvComputeStereoParamsForCameras(CvStereoCamera* stereoCamera) {
	/* For given intrinsic and extrinsic parameters computes rest parameters
	**   such as fundamental matrix. warping coeffs, epipoles, ...
	*/


	/* compute rotate matrix and translate vector */
	double rotMatr1[9];
	double rotMatr2[9];

	double transVect1[3];
	double transVect2[3];

	double convRotMatr[9];
	double convTransVect[3];

	/* fill matrices */
	icvCvt_32f_64d(stereoCamera->camera[0]->rotMatr, rotMatr1, 9);
	icvCvt_32f_64d(stereoCamera->camera[1]->rotMatr, rotMatr2, 9);

	icvCvt_32f_64d(stereoCamera->camera[0]->transVect, transVect1, 3);
	icvCvt_32f_64d(stereoCamera->camera[1]->transVect, transVect2, 3);

	icvCreateConvertMatrVect(   rotMatr1,
								transVect1,
								rotMatr2,
								transVect2,
								convRotMatr,
								convTransVect);

	/* copy to stereo camera params */
	icvCvt_64d_32f(convRotMatr, stereoCamera->rotMatrix, 9);
	icvCvt_64d_32f(convTransVect, stereoCamera->transVector, 3);


	icvGetQuadsTransformStruct(stereoCamera);
	icvComputeRestStereoParams(stereoCamera);
}



/*---------------------------------------------------------------------------------------*/

/* Get cut line for one image */
void icvGetCutPiece(   CvVect64d areaLineCoef1, CvVect64d areaLineCoef2,
					   CvPoint2D64d epipole,
					   CvSize imageSize,
					   CvPoint2D64d* point11, CvPoint2D64d* point12,
					   CvPoint2D64d* point21, CvPoint2D64d* point22,
					   int* result) {
	/* Compute nearest cut line to epipole */
	/* Get corners inside sector */
	/* Collect all candidate point */

	CvPoint2D64d candPoints[8];
	CvPoint2D64d midPoint;
	int numPoints = 0;
	int res;
	int i;

	double cutLine1[3];
	double cutLine2[3];

	/* Find middle line of sector */
	double midLine[3] = {0, 0, 0};


	/* Different way  */
	CvPoint2D64d pointOnLine1;  pointOnLine1.x = pointOnLine1.y = 0;
	CvPoint2D64d pointOnLine2;  pointOnLine2.x = pointOnLine2.y = 0;

	CvPoint2D64d start1, end1;

	icvGetCrossRectDirect( imageSize,
						   areaLineCoef1[0], areaLineCoef1[1], areaLineCoef1[2],
						   &start1, &end1, &res);
	if ( res > 0 ) {
		pointOnLine1 = start1;
	}

	icvGetCrossRectDirect( imageSize,
						   areaLineCoef2[0], areaLineCoef2[1], areaLineCoef2[2],
						   &start1, &end1, &res);
	if ( res > 0 ) {
		pointOnLine2 = start1;
	}

	icvGetMiddleAnglePoint(epipole, pointOnLine1, pointOnLine2, &midPoint);

	icvGetCoefForPiece(epipole, midPoint, &midLine[0], &midLine[1], &midLine[2], &res);

	/* Test corner points */
	CvPoint2D64d cornerPoint;
	CvPoint2D64d tmpPoints[2];

	cornerPoint.x = 0;
	cornerPoint.y = 0;
	icvTestPoint( cornerPoint, areaLineCoef1, areaLineCoef2, epipole, &res);
	if ( res == 1 ) {
		/* Add point */
		candPoints[numPoints] = cornerPoint;
		numPoints++;
	}

	cornerPoint.x = imageSize.width;
	cornerPoint.y = 0;
	icvTestPoint( cornerPoint, areaLineCoef1, areaLineCoef2, epipole, &res);
	if ( res == 1 ) {
		/* Add point */
		candPoints[numPoints] = cornerPoint;
		numPoints++;
	}

	cornerPoint.x = imageSize.width;
	cornerPoint.y = imageSize.height;
	icvTestPoint( cornerPoint, areaLineCoef1, areaLineCoef2, epipole, &res);
	if ( res == 1 ) {
		/* Add point */
		candPoints[numPoints] = cornerPoint;
		numPoints++;
	}

	cornerPoint.x = 0;
	cornerPoint.y = imageSize.height;
	icvTestPoint( cornerPoint, areaLineCoef1, areaLineCoef2, epipole, &res);
	if ( res == 1 ) {
		/* Add point */
		candPoints[numPoints] = cornerPoint;
		numPoints++;
	}

	/* Find cross line 1 with image border */
	icvGetCrossRectDirect( imageSize,
						   areaLineCoef1[0], areaLineCoef1[1], areaLineCoef1[2],
						   &tmpPoints[0], &tmpPoints[1],
						   &res);
	for ( i = 0; i < res; i++ ) {
		candPoints[numPoints++] = tmpPoints[i];
	}

	/* Find cross line 2 with image border */
	icvGetCrossRectDirect( imageSize,
						   areaLineCoef2[0], areaLineCoef2[1], areaLineCoef2[2],
						   &tmpPoints[0], &tmpPoints[1],
						   &res);

	for ( i = 0; i < res; i++ ) {
		candPoints[numPoints++] = tmpPoints[i];
	}

	if ( numPoints < 2 ) {
		*result = 0;
		return;/* Error. Not enought points */
	}
	/* Project all points to middle line and get max and min */

	CvPoint2D64d projPoint;
	CvPoint2D64d minPoint; minPoint.x = minPoint.y = FLT_MAX;
	CvPoint2D64d maxPoint; maxPoint.x = maxPoint.y = -FLT_MAX;


	double dist;
	double maxDist = 0;
	double minDist = 10000000;


	for ( i = 0; i < numPoints; i++ ) {
		icvProjectPointToDirect(candPoints[i], midLine, &projPoint);
		icvGetPieceLength(epipole, projPoint, &dist);
		if ( dist < minDist) {
			minDist = dist;
			minPoint = projPoint;
		}

		if ( dist > maxDist) {
			maxDist = dist;
			maxPoint = projPoint;
		}
	}

	/* We know maximum and minimum points. Now we can compute cut lines */

	icvGetNormalDirect(midLine, minPoint, cutLine1);
	icvGetNormalDirect(midLine, maxPoint, cutLine2);

	/* Test for begin of line. */
	CvPoint2D64d tmpPoint2;

	/* Get cross with */
	icvGetCrossDirectDirect(areaLineCoef1, cutLine1, point11, &res);
	icvGetCrossDirectDirect(areaLineCoef2, cutLine1, point12, &res);

	icvGetCrossDirectDirect(areaLineCoef1, cutLine2, point21, &res);
	icvGetCrossDirectDirect(areaLineCoef2, cutLine2, point22, &res);

	if ( epipole.x > imageSize.width * 0.5 ) {
		/* Need to change points */
		tmpPoint2 = *point11;
		*point11 = *point21;
		*point21 = tmpPoint2;

		tmpPoint2 = *point12;
		*point12 = *point22;
		*point22 = tmpPoint2;
	}

	return;
}
/*---------------------------------------------------------------------------------------*/
/* Get middle angle */
void icvGetMiddleAnglePoint(   CvPoint2D64d basePoint,
							   CvPoint2D64d point1, CvPoint2D64d point2,
							   CvPoint2D64d* midPoint) {
	/* !!! May be need to return error */

	double dist1;
	double dist2;
	icvGetPieceLength(basePoint, point1, &dist1);
	icvGetPieceLength(basePoint, point2, &dist2);
	CvPoint2D64d pointNew1;
	CvPoint2D64d pointNew2;
	double alpha = dist2 / dist1;

	pointNew1.x = basePoint.x + (1.0 / alpha) * ( point2.x - basePoint.x );
	pointNew1.y = basePoint.y + (1.0 / alpha) * ( point2.y - basePoint.y );

	pointNew2.x = basePoint.x + alpha * ( point1.x - basePoint.x );
	pointNew2.y = basePoint.y + alpha * ( point1.y - basePoint.y );

	int res;
	icvGetCrossPiecePiece(point1, point2, pointNew1, pointNew2, midPoint, &res);

	return;
}

/*---------------------------------------------------------------------------------------*/
/* Get normal direct to direct in line */
void icvGetNormalDirect(CvVect64d direct, CvPoint2D64d point, CvVect64d normDirect) {
	normDirect[0] =   direct[1];
	normDirect[1] = - direct[0];
	normDirect[2] = -(normDirect[0] * point.x + normDirect[1] * point.y);
	return;
}

/*---------------------------------------------------------------------------------------*/
CV_IMPL double icvGetVect(CvPoint2D64d basePoint, CvPoint2D64d point1, CvPoint2D64d point2) {
	return  (point1.x - basePoint.x) * (point2.y - basePoint.y) -
			(point2.x - basePoint.x) * (point1.y - basePoint.y);
}
/*---------------------------------------------------------------------------------------*/
/* Test for point in sector           */
/* Return 0 - point not inside sector */
/* Return 1 - point inside sector     */
void icvTestPoint( CvPoint2D64d testPoint,
				   CvVect64d line1, CvVect64d line2,
				   CvPoint2D64d basePoint,
				   int* result) {
	CvPoint2D64d point1, point2;

	icvProjectPointToDirect(testPoint, line1, &point1);
	icvProjectPointToDirect(testPoint, line2, &point2);

	double sign1 = icvGetVect(basePoint, point1, point2);
	double sign2 = icvGetVect(basePoint, point1, testPoint);
	if ( sign1 * sign2 > 0 ) {
		/* Correct for first line */
		sign1 = - sign1;
		sign2 = icvGetVect(basePoint, point2, testPoint);
		if ( sign1 * sign2 > 0 ) {
			/* Correct for both lines */
			*result = 1;
		} else {
			*result = 0;
		}
	} else {
		*result = 0;
	}

	return;
}

/*---------------------------------------------------------------------------------------*/
/* Project point to line */
void icvProjectPointToDirect(  CvPoint2D64d point, CvVect64d lineCoeff,
							   CvPoint2D64d* projectPoint) {
	double a = lineCoeff[0];
	double b = lineCoeff[1];

	double det =  1.0 / ( a * a + b * b );
	double delta =  a * point.y - b * point.x;

	projectPoint->x = ( -a * lineCoeff[2] - b * delta ) * det;
	projectPoint->y = ( -b * lineCoeff[2] + a * delta ) * det ;

	return;
}

/*---------------------------------------------------------------------------------------*/
/* Get distance from point to direction */
void icvGetDistanceFromPointToDirect( CvPoint2D64d point, CvVect64d lineCoef, double* dist) {
	CvPoint2D64d tmpPoint;
	icvProjectPointToDirect(point, lineCoef, &tmpPoint);
	double dx = point.x - tmpPoint.x;
	double dy = point.y - tmpPoint.y;
	*dist = sqrt(dx * dx + dy * dy);
	return;
}
/*---------------------------------------------------------------------------------------*/

CV_IMPL IplImage* icvCreateIsometricImage( IplImage* src, IplImage* dst,
		int desired_depth, int desired_num_channels ) {
	CvSize src_size ;
	src_size.width = src->width;
	src_size.height = src->height;

	CvSize dst_size = src_size;

	if ( dst ) {
		dst_size.width = dst->width;
		dst_size.height = dst->height;
	}

	if ( !dst || dst->depth != desired_depth ||
			dst->nChannels != desired_num_channels ||
			dst_size.width != src_size.width ||
			dst_size.height != dst_size.height ) {
		cvReleaseImage( &dst );
		dst = cvCreateImage( src_size, desired_depth, desired_num_channels );
		CvRect rect = cvRect(0, 0, src_size.width, src_size.height);
		cvSetImageROI( dst, rect );

	}

	return dst;
}

int
icvCvt_32f_64d( float* src, double* dst, int size ) {
	int t;

	if ( !src || !dst ) {
		return CV_NULLPTR_ERR;
	}
	if ( size <= 0 ) {
		return CV_BADRANGE_ERR;
	}

	for ( t = 0; t < size; t++ ) {
		dst[t] = (double) (src[t]);
	}

	return CV_OK;
}

/*======================================================================================*/
/* Type conversion double -> float */
int
icvCvt_64d_32f( double* src, float* dst, int size ) {
	int t;

	if ( !src || !dst ) {
		return CV_NULLPTR_ERR;
	}
	if ( size <= 0 ) {
		return CV_BADRANGE_ERR;
	}

	for ( t = 0; t < size; t++ ) {
		dst[t] = (float) (src[t]);
	}

	return CV_OK;
}

/*----------------------------------------------------------------------------------*/


/* Find line which cross frame by line(a,b,c) */
void FindLineForEpiline(    CvSize imageSize,
							float a, float b, float c,
							CvPoint2D32f* start, CvPoint2D32f* end,
							int* result) {
	result = result;
	CvPoint2D32f frameBeg;

	CvPoint2D32f frameEnd;
	CvPoint2D32f cross[4];
	int     haveCross[4];
	float   dist;

	haveCross[0] = 0;
	haveCross[1] = 0;
	haveCross[2] = 0;
	haveCross[3] = 0;

	frameBeg.x = 0;
	frameBeg.y = 0;
	frameEnd.x = (float)(imageSize.width);
	frameEnd.y = 0;
	haveCross[0] = icvGetCrossLineDirect(frameBeg, frameEnd, a, b, c, &cross[0]);

	frameBeg.x = (float)(imageSize.width);
	frameBeg.y = 0;
	frameEnd.x = (float)(imageSize.width);
	frameEnd.y = (float)(imageSize.height);
	haveCross[1] = icvGetCrossLineDirect(frameBeg, frameEnd, a, b, c, &cross[1]);

	frameBeg.x = (float)(imageSize.width);
	frameBeg.y = (float)(imageSize.height);
	frameEnd.x = 0;
	frameEnd.y = (float)(imageSize.height);
	haveCross[2] = icvGetCrossLineDirect(frameBeg, frameEnd, a, b, c, &cross[2]);

	frameBeg.x = 0;
	frameBeg.y = (float)(imageSize.height);
	frameEnd.x = 0;
	frameEnd.y = 0;
	haveCross[3] = icvGetCrossLineDirect(frameBeg, frameEnd, a, b, c, &cross[3]);

	int n;
	float minDist = (float)(INT_MAX);
	float maxDist = (float)(INT_MIN);

	int maxN = -1;
	int minN = -1;

	double midPointX = imageSize.width  / 2.0;
	double midPointY = imageSize.height / 2.0;

	for ( n = 0; n < 4; n++ ) {
		if ( haveCross[n] > 0 ) {
			dist =  (float)((midPointX - cross[n].x) * (midPointX - cross[n].x) +
							(midPointY - cross[n].y) * (midPointY - cross[n].y));

			if ( dist < minDist ) {
				minDist = dist;
				minN = n;
			}

			if ( dist > maxDist ) {
				maxDist = dist;
				maxN = n;
			}
		}
	}

	if ( minN >= 0 && maxN >= 0 && (minN != maxN) ) {
		*start = cross[minN];
		*end   = cross[maxN];
	} else {
		start->x = 0;
		start->y = 0;
		end->x = 0;
		end->y = 0;
	}

	return;

}


/*----------------------------------------------------------------------------------*/

int GetAngleLinee( CvPoint2D32f epipole, CvSize imageSize, CvPoint2D32f point1, CvPoint2D32f point2) {
	float width  = (float)(imageSize.width);
	float height = (float)(imageSize.height);

	/* Get crosslines with image corners */

	/* Find four lines */

	CvPoint2D32f pa, pb, pc, pd;

	pa.x = 0;
	pa.y = 0;

	pb.x = width;
	pb.y = 0;

	pd.x = width;
	pd.y = height;

	pc.x = 0;
	pc.y = height;

	/* We can compute points for angle */
	/* Test for place section */

	float x, y;
	x = epipole.x;
	y = epipole.y;

	if ( x < 0 ) {
		/* 1,4,7 */
		if ( y < 0) {
			/* 1 */
			point1 = pb;
			point2 = pc;
		} else if ( y > height ) {
			/* 7 */
			point1 = pa;
			point2 = pd;
		} else {
			/* 4 */
			point1 = pa;
			point2 = pc;
		}
	} else if ( x > width ) {
		/* 3,6,9 */
		if ( y < 0 ) {
			/* 3 */
			point1 = pa;
			point2 = pd;
		} else if ( y > height ) {
			/* 9 */
			point1 = pc;
			point2 = pb;
		} else {
			/* 6 */
			point1 = pb;
			point2 = pd;
		}
	} else {
		/* 2,5,8 */
		if ( y < 0 ) {
			/* 2 */
			point1 = pa;
			point2 = pb;
		} else if ( y > height ) {
			/* 8 */
			point1 = pc;
			point2 = pd;
		} else {
			/* 5 - point in the image */
			return 2;
		}


	}


	return 0;
}

/*--------------------------------------------------------------------------------------*/
void icvComputePerspectiveCoeffs(const CvPoint2D32f srcQuad[4], const CvPoint2D32f dstQuad[4], double coeffs[3][3]) {
	/* Computes perspective coeffs for transformation from src to dst quad */


	CV_FUNCNAME( "icvComputePerspectiveCoeffs" );

	__BEGIN__;

	double A[64];
	double b[8];
	double c[8];
	CvPoint2D32f pt[4];
	int i;

	pt[0] = srcQuad[0];
	pt[1] = srcQuad[1];
	pt[2] = srcQuad[2];
	pt[3] = srcQuad[3];

	for ( i = 0; i < 4; i++ ) {
#if 0
		double x = dstQuad[i].x;
		double y = dstQuad[i].y;
		double X = pt[i].x;
		double Y = pt[i].y;
#else
		double x = pt[i].x;
		double y = pt[i].y;
		double X = dstQuad[i].x;
		double Y = dstQuad[i].y;
#endif
		double* a = A + i * 16;

		a[0] = x;
		a[1] = y;
		a[2] = 1;
		a[3] = 0;
		a[4] = 0;
		a[5] = 0;
		a[6] = -X * x;
		a[7] = -X * y;

		a += 8;

		a[0] = 0;
		a[1] = 0;
		a[2] = 0;
		a[3] = x;
		a[4] = y;
		a[5] = 1;
		a[6] = -Y * x;
		a[7] = -Y * y;

		b[i*2] = X;
		b[i*2 + 1] = Y;
	}

	{
		double invA[64];
		CvMat matA = cvMat( 8, 8, CV_64F, A );
		CvMat matInvA = cvMat( 8, 8, CV_64F, invA );
		CvMat matB = cvMat( 8, 1, CV_64F, b );
		CvMat matX = cvMat( 8, 1, CV_64F, c );

		CV_CALL( cvPseudoInverse( &matA, &matInvA ));
		CV_CALL( cvMatMulAdd( &matInvA, &matB, 0, &matX ));
	}

	coeffs[0][0] = c[0];
	coeffs[0][1] = c[1];
	coeffs[0][2] = c[2];
	coeffs[1][0] = c[3];
	coeffs[1][1] = c[4];
	coeffs[1][2] = c[5];
	coeffs[2][0] = c[6];
	coeffs[2][1] = c[7];
	coeffs[2][2] = 1.0;

	__END__;

	return;
}

/*--------------------------------------------------------------------------------------*/

CV_IMPL void cvComputePerspectiveMap(const double c[3][3], CvArr* rectMapX, CvArr* rectMapY ) {
	CV_FUNCNAME( "cvComputePerspectiveMap" );

	__BEGIN__;

	CvSize size;
	CvMat  stubx, *mapx = (CvMat*)rectMapX;
	CvMat  stuby, *mapy = (CvMat*)rectMapY;
	int i, j;

	CV_CALL( mapx = cvGetMat( mapx, &stubx ));
	CV_CALL( mapy = cvGetMat( mapy, &stuby ));

	if ( CV_MAT_TYPE( mapx->type ) != CV_32FC1 || CV_MAT_TYPE( mapy->type ) != CV_32FC1 ) {
		CV_ERROR( CV_StsUnsupportedFormat, "" );
	}

	size = cvGetMatSize(mapx);
	assert( fabs(c[2][2] - 1.) < FLT_EPSILON );

	for ( i = 0; i < size.height; i++ ) {
		float* mx = (float*)(mapx->data.ptr + mapx->step * i);
		float* my = (float*)(mapy->data.ptr + mapy->step * i);

		for ( j = 0; j < size.width; j++ ) {
			double w = 1. / (c[2][0] * j + c[2][1] * i + 1.);
			double x = (c[0][0] * j + c[0][1] * i + c[0][2]) * w;
			double y = (c[1][0] * j + c[1][1] * i + c[1][2]) * w;

			mx[j] = (float)x;
			my[j] = (float)y;
		}
	}

	__END__;
}

/*--------------------------------------------------------------------------------------*/

CV_IMPL void cvInitPerspectiveTransform( CvSize size, const CvPoint2D32f quad[4], double matrix[3][3],
		CvArr* rectMap ) {
	/* Computes Perspective Transform coeffs and map if need
	    for given image size and given result quad */
	CV_FUNCNAME( "cvInitPerspectiveTransform" );

	__BEGIN__;

	double A[64];
	double b[8];
	double c[8];
	CvPoint2D32f pt[4];
	CvMat  mapstub, *map = (CvMat*)rectMap;
	int i, j;

	if ( map ) {
		CV_CALL( map = cvGetMat( map, &mapstub ));

		if ( CV_MAT_TYPE( map->type ) != CV_32FC2 ) {
			CV_ERROR( CV_StsUnsupportedFormat, "" );
		}

		if ( map->width != size.width || map->height != size.height ) {
			CV_ERROR( CV_StsUnmatchedSizes, "" );
		}
	}

	pt[0] = cvPoint2D32f( 0, 0 );
	pt[1] = cvPoint2D32f( size.width, 0 );
	pt[2] = cvPoint2D32f( size.width, size.height );
	pt[3] = cvPoint2D32f( 0, size.height );

	for ( i = 0; i < 4; i++ ) {
#if 0
		double x = quad[i].x;
		double y = quad[i].y;
		double X = pt[i].x;
		double Y = pt[i].y;
#else
		double x = pt[i].x;
		double y = pt[i].y;
		double X = quad[i].x;
		double Y = quad[i].y;
#endif
		double* a = A + i * 16;

		a[0] = x;
		a[1] = y;
		a[2] = 1;
		a[3] = 0;
		a[4] = 0;
		a[5] = 0;
		a[6] = -X * x;
		a[7] = -X * y;

		a += 8;

		a[0] = 0;
		a[1] = 0;
		a[2] = 0;
		a[3] = x;
		a[4] = y;
		a[5] = 1;
		a[6] = -Y * x;
		a[7] = -Y * y;

		b[i*2] = X;
		b[i*2 + 1] = Y;
	}

	{
		double invA[64];
		CvMat matA = cvMat( 8, 8, CV_64F, A );
		CvMat matInvA = cvMat( 8, 8, CV_64F, invA );
		CvMat matB = cvMat( 8, 1, CV_64F, b );
		CvMat matX = cvMat( 8, 1, CV_64F, c );

		CV_CALL( cvPseudoInverse( &matA, &matInvA ));
		CV_CALL( cvMatMulAdd( &matInvA, &matB, 0, &matX ));
	}

	matrix[0][0] = c[0];
	matrix[0][1] = c[1];
	matrix[0][2] = c[2];
	matrix[1][0] = c[3];
	matrix[1][1] = c[4];
	matrix[1][2] = c[5];
	matrix[2][0] = c[6];
	matrix[2][1] = c[7];
	matrix[2][2] = 1.0;

	if ( map ) {
		for ( i = 0; i < size.height; i++ ) {
			CvPoint2D32f* maprow = (CvPoint2D32f*)(map->data.ptr + map->step * i);
			for ( j = 0; j < size.width; j++ ) {
				double w = 1. / (c[6] * j + c[7] * i + 1.);
				double x = (c[0] * j + c[1] * i + c[2]) * w;
				double y = (c[3] * j + c[4] * i + c[5]) * w;

				maprow[j].x = (float)x;
				maprow[j].y = (float)y;
			}
		}
	}

	__END__;

	return;
}


/*-----------------------------------------------------------------------*/
/* Compute projected infinite point for second image if first image point is known */
void icvComputeeInfiniteProject1(   CvMatr64d     rotMatr,
									CvMatr64d     camMatr1,
									CvMatr64d     camMatr2,
									CvPoint2D32f  point1,
									CvPoint2D32f* point2) {
	double invMatr1[9];
	icvInvertMatrix_64d(camMatr1, 3, invMatr1);
	double P1[3];
	double p1[3];
	p1[0] = (double)(point1.x);
	p1[1] = (double)(point1.y);
	p1[2] = 1;

	icvMulMatrix_64d(   invMatr1,
						3, 3,
						p1,
						1, 3,
						P1);

	double invR[9];
	icvTransposeMatrix_64d( rotMatr, 3, 3, invR );

	/* Change system 1 to system 2 */
	double P2[3];
	icvMulMatrix_64d(   invR,
						3, 3,
						P1,
						1, 3,
						P2);

	/* Now we can project this point to image 2 */
	double projP[3];

	icvMulMatrix_64d(   camMatr2,
						3, 3,
						P2,
						1, 3,
						projP);

	point2->x = (float)(projP[0] / projP[2]);
	point2->y = (float)(projP[1] / projP[2]);

	return;
}

/*-----------------------------------------------------------------------*/
/* Compute projected infinite point for second image if first image point is known */
void icvComputeeInfiniteProject2(   CvMatr64d     rotMatr,
									CvMatr64d     camMatr1,
									CvMatr64d     camMatr2,
									CvPoint2D32f*  point1,
									CvPoint2D32f point2) {
	double invMatr2[9];
	icvInvertMatrix_64d(camMatr2, 3, invMatr2);
	double P2[3];
	double p2[3];
	p2[0] = (double)(point2.x);
	p2[1] = (double)(point2.y);
	p2[2] = 1;

	icvMulMatrix_64d(   invMatr2,
						3, 3,
						p2,
						1, 3,
						P2);

	/* Change system 1 to system 2 */

	double P1[3];
	icvMulMatrix_64d(   rotMatr,
						3, 3,
						P2,
						1, 3,
						P1);

	/* Now we can project this point to image 2 */
	double projP[3];

	icvMulMatrix_64d(   camMatr1,
						3, 3,
						P1,
						1, 3,
						projP);

	point1->x = (float)(projP[0] / projP[2]);
	point1->y = (float)(projP[1] / projP[2]);

	return;
}

/* Select best R and t for given cameras, points, ... */
/* For both cameras */
int icvSelectBestRt(           int           numImages,
							   int*          numPoints,
							   CvPoint2D32f* imagePoints1,
							   CvPoint2D32f* imagePoints2,
							   CvPoint3D32f* objectPoints,

							   CvMatr32f     cameraMatrix1,
							   CvVect32f     distortion1,
							   CvMatr32f     rotMatrs1,
							   CvVect32f     transVects1,

							   CvMatr32f     cameraMatrix2,
							   CvVect32f     distortion2,
							   CvMatr32f     rotMatrs2,
							   CvVect32f     transVects2,

							   CvMatr32f     bestRotMatr,
							   CvVect32f     bestTransVect
				   ) {

	/* Need to convert input data 32 -> 64 */
	CvPoint3D64d* objectPoints_64d;

	double* rotMatrs1_64d;
	double* rotMatrs2_64d;

	double* transVects1_64d;
	double* transVects2_64d;

	double cameraMatrix1_64d[9];
	double cameraMatrix2_64d[9];

	double distortion1_64d[4];
	double distortion2_64d[4];

	/* allocate memory for 64d data */
	int totalNum = 0;

	int i;
	for ( i = 0; i < numImages; i++ ) {
		totalNum += numPoints[i];
	}

	objectPoints_64d = (CvPoint3D64d*)calloc(totalNum, sizeof(CvPoint3D64d));

	rotMatrs1_64d    = (double*)calloc(numImages, sizeof(double) * 9);
	rotMatrs2_64d    = (double*)calloc(numImages, sizeof(double) * 9);

	transVects1_64d  = (double*)calloc(numImages, sizeof(double) * 3);
	transVects2_64d  = (double*)calloc(numImages, sizeof(double) * 3);

	/* Convert input data to 64d */

	icvCvt_32f_64d((float*)objectPoints, (double*)objectPoints_64d,  totalNum * 3);

	icvCvt_32f_64d(rotMatrs1, rotMatrs1_64d,  numImages * 9);
	icvCvt_32f_64d(rotMatrs2, rotMatrs2_64d,  numImages * 9);

	icvCvt_32f_64d(transVects1, transVects1_64d,  numImages * 3);
	icvCvt_32f_64d(transVects2, transVects2_64d,  numImages * 3);

	/* Convert to arrays */
	icvCvt_32f_64d(cameraMatrix1, cameraMatrix1_64d, 9);
	icvCvt_32f_64d(cameraMatrix2, cameraMatrix2_64d, 9);

	icvCvt_32f_64d(distortion1, distortion1_64d, 4);
	icvCvt_32f_64d(distortion2, distortion2_64d, 4);


	/* for each R and t compute error for image pair */
	float* errors;

	errors = (float*)calloc(numImages * numImages, sizeof(float));
	if ( errors == 0 ) {
		return CV_OUTOFMEM_ERR;
	}

	int currImagePair;
	int currRt;
	for ( currRt = 0; currRt < numImages; currRt++ ) {
		int begPoint = 0;
		for (currImagePair = 0; currImagePair < numImages; currImagePair++ ) {
			/* For current R,t R,t compute relative position of cameras */

			double convRotMatr[9];
			double convTransVect[3];

			icvCreateConvertMatrVect( rotMatrs1_64d + currRt * 9,
									  transVects1_64d + currRt * 3,
									  rotMatrs2_64d + currRt * 9,
									  transVects2_64d + currRt * 3,
									  convRotMatr,
									  convTransVect);

			/* Project points using relative position of cameras */

			double convRotMatr2[9];
			double convTransVect2[3];

			convRotMatr2[0] = 1;
			convRotMatr2[1] = 0;
			convRotMatr2[2] = 0;

			convRotMatr2[3] = 0;
			convRotMatr2[4] = 1;
			convRotMatr2[5] = 0;

			convRotMatr2[6] = 0;
			convRotMatr2[7] = 0;
			convRotMatr2[8] = 1;

			convTransVect2[0] = 0;
			convTransVect2[1] = 0;
			convTransVect2[2] = 0;

			/* Compute error for given pair and Rt */
			/* We must project points to image and compute error */

			CvPoint2D64d* projImagePoints1;
			CvPoint2D64d* projImagePoints2;

			CvPoint3D64d* points1;
			CvPoint3D64d* points2;

			int numberPnt;
			numberPnt = numPoints[currImagePair];
			projImagePoints1 = (CvPoint2D64d*)calloc(numberPnt, sizeof(CvPoint2D64d));
			projImagePoints2 = (CvPoint2D64d*)calloc(numberPnt, sizeof(CvPoint2D64d));

			points1 = (CvPoint3D64d*)calloc(numberPnt, sizeof(CvPoint3D64d));
			points2 = (CvPoint3D64d*)calloc(numberPnt, sizeof(CvPoint3D64d));

			/* Transform object points to first camera position */
			int i;
			for ( i = 0; i < numberPnt; i++ ) {
				/* Create second camera point */
				CvPoint3D64d tmpPoint;
				tmpPoint.x = (double)(objectPoints[i].x);
				tmpPoint.y = (double)(objectPoints[i].y);
				tmpPoint.z = (double)(objectPoints[i].z);

				icvConvertPointSystem(  tmpPoint,
										points2 + i,
										rotMatrs2_64d + currImagePair * 9,
										transVects2_64d + currImagePair * 3);

				/* Create first camera point using R, t */
				icvConvertPointSystem(  points2[i],
										points1 + i,
										convRotMatr,
										convTransVect);

				CvPoint3D64d tmpPoint2 = { 0, 0, 0 };
				icvConvertPointSystem(  tmpPoint,
										&tmpPoint2,
										rotMatrs1_64d + currImagePair * 9,
										transVects1_64d + currImagePair * 3);
				double err;
				double dx, dy, dz;
				dx = tmpPoint2.x - points1[i].x;
				dy = tmpPoint2.y - points1[i].y;
				dz = tmpPoint2.z - points1[i].z;
				err = sqrt(dx * dx + dy * dy + dz * dz);


			}

#if 0
			cvProjectPointsSimple(  numPoints[currImagePair],
									objectPoints_64d + begPoint,
									rotMatrs1_64d + currRt * 9,
									transVects1_64d + currRt * 3,
									cameraMatrix1_64d,
									distortion1_64d,
									projImagePoints1);

			cvProjectPointsSimple(  numPoints[currImagePair],
									objectPoints_64d + begPoint,
									rotMatrs2_64d + currRt * 9,
									transVects2_64d + currRt * 3,
									cameraMatrix2_64d,
									distortion2_64d,
									projImagePoints2);
#endif

			/* Project with no translate and no rotation */

#if 0
			{
				double nodist[4] = {0, 0, 0, 0};
				cvProjectPointsSimple(  numPoints[currImagePair],
										points1,
										convRotMatr2,
										convTransVect2,
										cameraMatrix1_64d,
										nodist,
										projImagePoints1);

				cvProjectPointsSimple(  numPoints[currImagePair],
										points2,
										convRotMatr2,
										convTransVect2,
										cameraMatrix2_64d,
										nodist,
										projImagePoints2);

			}
#endif

			cvProjectPointsSimple(  numPoints[currImagePair],
									points1,
									convRotMatr2,
									convTransVect2,
									cameraMatrix1_64d,
									distortion1_64d,
									projImagePoints1);

			cvProjectPointsSimple(  numPoints[currImagePair],
									points2,
									convRotMatr2,
									convTransVect2,
									cameraMatrix2_64d,
									distortion2_64d,
									projImagePoints2);

			/* points are projected. Compute error */

			int currPoint;
			double err1 = 0;
			double err2 = 0;
			double err;
			for ( currPoint = 0; currPoint < numberPnt; currPoint++ ) {
				double len1, len2;
				double dx1, dy1;
				dx1 = imagePoints1[begPoint+currPoint].x - projImagePoints1[currPoint].x;
				dy1 = imagePoints1[begPoint+currPoint].y - projImagePoints1[currPoint].y;
				len1 = sqrt(dx1 * dx1 + dy1 * dy1);
				err1 += len1;

				double dx2, dy2;
				dx2 = imagePoints2[begPoint+currPoint].x - projImagePoints2[currPoint].x;
				dy2 = imagePoints2[begPoint+currPoint].y - projImagePoints2[currPoint].y;
				len2 = sqrt(dx2 * dx2 + dy2 * dy2);
				err2 += len2;
			}

			err1 /= (float)(numberPnt);
			err2 /= (float)(numberPnt);

			err = (err1 + err2) * 0.5;
			begPoint += numberPnt;

			/* Set this error to */
			errors[numImages* currImagePair+currRt] = (float)err;

			free(points1);
			free(points2);
			free(projImagePoints1);
			free(projImagePoints2);
		}
	}

	/* Just select R and t with minimal average error */

	int bestnumRt = 0;
	float minError = 0;/* Just for no warnings. Uses 'first' flag. */
	int first = 1;
	for ( currRt = 0; currRt < numImages; currRt++ ) {
		float avErr = 0;
		for (currImagePair = 0; currImagePair < numImages; currImagePair++ ) {
			avErr += errors[numImages*currImagePair+currRt];
		}
		avErr /= (float)(numImages);

		if ( first ) {
			bestnumRt = 0;
			minError = avErr;
			first = 0;
		} else {
			if ( avErr < minError ) {
				bestnumRt = currRt;
				minError = avErr;
			}
		}

	}

	double bestRotMatr_64d[9];
	double bestTransVect_64d[3];

	icvCreateConvertMatrVect( rotMatrs1_64d + bestnumRt * 9,
							  transVects1_64d + bestnumRt * 3,
							  rotMatrs2_64d + bestnumRt * 9,
							  transVects2_64d + bestnumRt * 3,
							  bestRotMatr_64d,
							  bestTransVect_64d);

	icvCvt_64d_32f(bestRotMatr_64d, bestRotMatr, 9);
	icvCvt_64d_32f(bestTransVect_64d, bestTransVect, 3);


	free(errors);

	return CV_OK;
}


/* ----------------- Stereo calibration functions --------------------- */

float icvDefinePointPosition(CvPoint2D32f point1, CvPoint2D32f point2, CvPoint2D32f point) {
	float ax = point2.x - point1.x;
	float ay = point2.y - point1.y;

	float bx = point.x - point1.x;
	float by = point.y - point1.y;

	return (ax * by - ay * bx);
}

/* Convert function for stereo warping */
int icvConvertWarpCoordinates(double coeffs[3][3],
							  CvPoint2D32f* cameraPoint,
							  CvPoint2D32f* warpPoint,
							  int direction) {
	double x, y;
	double det;
	if ( direction == CV_WARP_TO_CAMERA ) {
		/* convert from camera image to warped image coordinates */
		x = warpPoint->x;
		y = warpPoint->y;

		det = (coeffs[2][0] * x + coeffs[2][1] * y + coeffs[2][2]);
		if ( fabs(det) > 1e-8 ) {
			cameraPoint->x = (float)((coeffs[0][0] * x + coeffs[0][1] * y + coeffs[0][2]) / det);
			cameraPoint->y = (float)((coeffs[1][0] * x + coeffs[1][1] * y + coeffs[1][2]) / det);
			return CV_OK;
		}
	} else if ( direction == CV_CAMERA_TO_WARP ) {
		/* convert from warped image to camera image coordinates */
		x = cameraPoint->x;
		y = cameraPoint->y;

		det = (coeffs[2][0] * x - coeffs[0][0]) * (coeffs[2][1] * y - coeffs[1][1]) - (coeffs[2][1] * x - coeffs[0][1]) * (coeffs[2][0] * y - coeffs[1][0]);

		if ( fabs(det) > 1e-8 ) {
			warpPoint->x = (float)(((coeffs[0][2] - coeffs[2][2] * x) * (coeffs[2][1] * y - coeffs[1][1]) - (coeffs[2][1] * x - coeffs[0][1]) * (coeffs[1][2] - coeffs[2][2] * y)) / det);
			warpPoint->y = (float)(((coeffs[2][0] * x - coeffs[0][0]) * (coeffs[1][2] - coeffs[2][2] * y) - (coeffs[0][2] - coeffs[2][2] * x) * (coeffs[2][0] * y - coeffs[1][0])) / det);
			return CV_OK;
		}
	}

	return CV_BADFACTOR_ERR;
}

/* Compute stereo params using some camera params */
/* by Valery Mosyagin. int ComputeRestStereoParams(StereoParams *stereoparams) */
int icvComputeRestStereoParams(CvStereoCamera* stereoparams) {


	icvGetQuadsTransformStruct(stereoparams);

	cvInitPerspectiveTransform( stereoparams->warpSize,
								stereoparams->quad[0],
								stereoparams->coeffs[0],
								0);

	cvInitPerspectiveTransform( stereoparams->warpSize,
								stereoparams->quad[1],
								stereoparams->coeffs[1],
								0);

	/* Create border for warped images */
	CvPoint2D32f corns[4];
	corns[0].x = 0;
	corns[0].y = 0;

	corns[1].x = (float)(stereoparams->camera[0]->imgSize[0] - 1);
	corns[1].y = 0;

	corns[2].x = (float)(stereoparams->camera[0]->imgSize[0] - 1);
	corns[2].y = (float)(stereoparams->camera[0]->imgSize[1] - 1);

	corns[3].x = 0;
	corns[3].y = (float)(stereoparams->camera[0]->imgSize[1] - 1);

	int i;
	for ( i = 0; i < 4; i++ ) {
		/* For first camera */
		icvConvertWarpCoordinates( stereoparams->coeffs[0],
								   corns + i,
								   stereoparams->border[0] + i,
								   CV_CAMERA_TO_WARP);

		/* For second camera */
		icvConvertWarpCoordinates( stereoparams->coeffs[1],
								   corns + i,
								   stereoparams->border[1] + i,
								   CV_CAMERA_TO_WARP);
	}

	/* Test compute  */
	{
		CvPoint2D32f warpPoints[4];
		warpPoints[0] = cvPoint2D32f(0, 0);
		warpPoints[1] = cvPoint2D32f(stereoparams->warpSize.width - 1, 0);
		warpPoints[2] = cvPoint2D32f(stereoparams->warpSize.width - 1, stereoparams->warpSize.height - 1);
		warpPoints[3] = cvPoint2D32f(0, stereoparams->warpSize.height - 1);

		CvPoint2D32f camPoints1[4];
		CvPoint2D32f camPoints2[4];

		for ( int i = 0; i < 4; i++ ) {
			icvConvertWarpCoordinates(stereoparams->coeffs[0],
									  camPoints1 + i,
									  warpPoints + i,
									  CV_WARP_TO_CAMERA);

			icvConvertWarpCoordinates(stereoparams->coeffs[1],
									  camPoints2 + i,
									  warpPoints + i,
									  CV_WARP_TO_CAMERA);
		}
	}


	/* Allocate memory for scanlines coeffs */

	stereoparams->lineCoeffs = (CvStereoLineCoeff*)calloc(stereoparams->warpSize.height, sizeof(CvStereoLineCoeff));

	/* Compute coeffs for epilines  */

	icvComputeCoeffForStereo( stereoparams);

	/* all coeffs are known */
	return CV_OK;
}

/*-------------------------------------------------------------------------------------------*/

int icvStereoCalibration( int numImages,
						  int* nums,
						  CvSize imageSize,
						  CvPoint2D32f* imagePoints1,
						  CvPoint2D32f* imagePoints2,
						  CvPoint3D32f* objectPoints,
						  CvStereoCamera* stereoparams
						) {
	/* Firstly we must calibrate both cameras */
	/*  Alocate memory for data */
	/* Allocate for translate vectors */
	float* transVects1;
	float* transVects2;
	float* rotMatrs1;
	float* rotMatrs2;

	transVects1 = (float*)calloc(numImages, sizeof(float) * 3);
	transVects2 = (float*)calloc(numImages, sizeof(float) * 3);

	rotMatrs1   = (float*)calloc(numImages, sizeof(float) * 9);
	rotMatrs2   = (float*)calloc(numImages, sizeof(float) * 9);

	/* Calibrate first camera */
	cvCalibrateCamera(  numImages,
						nums,
						imageSize,
						imagePoints1,
						objectPoints,
						stereoparams->camera[0]->distortion,
						stereoparams->camera[0]->matrix,
						transVects1,
						rotMatrs1,
						1);

	/* Calibrate second camera */
	cvCalibrateCamera(  numImages,
						nums,
						imageSize,
						imagePoints2,
						objectPoints,
						stereoparams->camera[1]->distortion,
						stereoparams->camera[1]->matrix,
						transVects2,
						rotMatrs2,
						1);

	/* Cameras are calibrated */

	stereoparams->camera[0]->imgSize[0] = (float)imageSize.width;
	stereoparams->camera[0]->imgSize[1] = (float)imageSize.height;

	stereoparams->camera[1]->imgSize[0] = (float)imageSize.width;
	stereoparams->camera[1]->imgSize[1] = (float)imageSize.height;

	icvSelectBestRt(    numImages,
						nums,
						imagePoints1,
						imagePoints2,
						objectPoints,
						stereoparams->camera[0]->matrix,
						stereoparams->camera[0]->distortion,
						rotMatrs1,
						transVects1,
						stereoparams->camera[1]->matrix,
						stereoparams->camera[1]->distortion,
						rotMatrs2,
						transVects2,
						stereoparams->rotMatrix,
						stereoparams->transVector
				   );

	/* Free memory */
	free(transVects1);
	free(transVects2);
	free(rotMatrs1);
	free(rotMatrs2);

	icvComputeRestStereoParams(stereoparams);

	return CV_NO_ERR;
}

/* Find line from epipole */
void FindLine(CvPoint2D32f epipole, CvSize imageSize, CvPoint2D32f point, CvPoint2D32f* start, CvPoint2D32f* end) {
	CvPoint2D32f frameBeg;
	CvPoint2D32f frameEnd;
	CvPoint2D32f cross[4];
	int     haveCross[4];
	float   dist;

	haveCross[0] = 0;
	haveCross[1] = 0;
	haveCross[2] = 0;
	haveCross[3] = 0;

	frameBeg.x = 0;
	frameBeg.y = 0;
	frameEnd.x = (float)(imageSize.width);
	frameEnd.y = 0;
	haveCross[0] = icvGetCrossPieceVector(frameBeg, frameEnd, epipole, point, &cross[0]);

	frameBeg.x = (float)(imageSize.width);
	frameBeg.y = 0;
	frameEnd.x = (float)(imageSize.width);
	frameEnd.y = (float)(imageSize.height);
	haveCross[1] = icvGetCrossPieceVector(frameBeg, frameEnd, epipole, point, &cross[1]);

	frameBeg.x = (float)(imageSize.width);
	frameBeg.y = (float)(imageSize.height);
	frameEnd.x = 0;
	frameEnd.y = (float)(imageSize.height);
	haveCross[2] = icvGetCrossPieceVector(frameBeg, frameEnd, epipole, point, &cross[2]);

	frameBeg.x = 0;
	frameBeg.y = (float)(imageSize.height);
	frameEnd.x = 0;
	frameEnd.y = 0;
	haveCross[3] = icvGetCrossPieceVector(frameBeg, frameEnd, epipole, point, &cross[3]);

	int n;
	float minDist = (float)(INT_MAX);
	float maxDist = (float)(INT_MIN);

	int maxN = -1;
	int minN = -1;

	for ( n = 0; n < 4; n++ ) {
		if ( haveCross[n] > 0 ) {
			dist =  (epipole.x - cross[n].x) * (epipole.x - cross[n].x) +
					(epipole.y - cross[n].y) * (epipole.y - cross[n].y);

			if ( dist < minDist ) {
				minDist = dist;
				minN = n;
			}

			if ( dist > maxDist ) {
				maxDist = dist;
				maxN = n;
			}
		}
	}

	if ( minN >= 0 && maxN >= 0 && (minN != maxN) ) {
		*start = cross[minN];
		*end   = cross[maxN];
	} else {
		start->x = 0;
		start->y = 0;
		end->x = 0;
		end->y = 0;
	}

	return;
}


/* Find line which cross frame by line(a,b,c) */
void FindLineForEpiline(CvSize imageSize, float a, float b, float c, CvPoint2D32f* start, CvPoint2D32f* end) {
	CvPoint2D32f frameBeg;
	CvPoint2D32f frameEnd;
	CvPoint2D32f cross[4];
	int     haveCross[4];
	float   dist;

	haveCross[0] = 0;
	haveCross[1] = 0;
	haveCross[2] = 0;
	haveCross[3] = 0;

	frameBeg.x = 0;
	frameBeg.y = 0;
	frameEnd.x = (float)(imageSize.width);
	frameEnd.y = 0;
	haveCross[0] = icvGetCrossLineDirect(frameBeg, frameEnd, a, b, c, &cross[0]);

	frameBeg.x = (float)(imageSize.width);
	frameBeg.y = 0;
	frameEnd.x = (float)(imageSize.width);
	frameEnd.y = (float)(imageSize.height);
	haveCross[1] = icvGetCrossLineDirect(frameBeg, frameEnd, a, b, c, &cross[1]);

	frameBeg.x = (float)(imageSize.width);
	frameBeg.y = (float)(imageSize.height);
	frameEnd.x = 0;
	frameEnd.y = (float)(imageSize.height);
	haveCross[2] = icvGetCrossLineDirect(frameBeg, frameEnd, a, b, c, &cross[2]);

	frameBeg.x = 0;
	frameBeg.y = (float)(imageSize.height);
	frameEnd.x = 0;
	frameEnd.y = 0;
	haveCross[3] = icvGetCrossLineDirect(frameBeg, frameEnd, a, b, c, &cross[3]);

	int n;
	float minDist = (float)(INT_MAX);
	float maxDist = (float)(INT_MIN);

	int maxN = -1;
	int minN = -1;

	double midPointX = imageSize.width  / 2.0;
	double midPointY = imageSize.height / 2.0;

	for ( n = 0; n < 4; n++ ) {
		if ( haveCross[n] > 0 ) {
			dist =  (float)((midPointX - cross[n].x) * (midPointX - cross[n].x) +
							(midPointY - cross[n].y) * (midPointY - cross[n].y));

			if ( dist < minDist ) {
				minDist = dist;
				minN = n;
			}

			if ( dist > maxDist ) {
				maxDist = dist;
				maxN = n;
			}
		}
	}

	if ( minN >= 0 && maxN >= 0 && (minN != maxN) ) {
		*start = cross[minN];
		*end   = cross[maxN];
	} else {
		start->x = 0;
		start->y = 0;
		end->x = 0;
		end->y = 0;
	}

	return;

}

/* Cross lines */
int GetCrossLines(CvPoint2D32f p1_start, CvPoint2D32f p1_end, CvPoint2D32f p2_start, CvPoint2D32f p2_end, CvPoint2D32f* cross) {
	double ex1, ey1, ex2, ey2;
	double px1, py1, px2, py2;
	double del;
	double delA, delB, delX, delY;
	double alpha, betta;

	ex1 = p1_start.x;
	ey1 = p1_start.y;
	ex2 = p1_end.x;
	ey2 = p1_end.y;

	px1 = p2_start.x;
	py1 = p2_start.y;
	px2 = p2_end.x;
	py2 = p2_end.y;

	del = (ex1 - ex2) * (py2 - py1) + (ey2 - ey1) * (px2 - px1);
	if ( del == 0) {
		return -1;
	}

	delA =  (px1 - ex1) * (py1 - py2) + (ey1 - py1) * (px1 - px2);
	delB =  (ex1 - px1) * (ey1 - ey2) + (py1 - ey1) * (ex1 - ex2);

	alpha =  delA / del;
	betta = -delB / del;

	if ( alpha < 0 || alpha > 1.0 || betta < 0 || betta > 1.0) {
		return -1;
	}

	delX =  (ex1 - ex2) * (py1 * (px1 - px2) - px1 * (py1 - py2)) +
			(px1 - px2) * (ex1 * (ey1 - ey2) - ey1 * (ex1 - ex2));

	delY =  (ey1 - ey2) * (px1 * (py1 - py2) - py1 * (px1 - px2)) +
			(py1 - py2) * (ey1 * (ex1 - ex2) - ex1 * (ey1 - ey2));

	cross->x = (float)( delX / del);
	cross->y = (float)(-delY / del);
	return 1;
}


int icvGetCrossPieceVector(CvPoint2D32f p1_start, CvPoint2D32f p1_end, CvPoint2D32f v2_start, CvPoint2D32f v2_end, CvPoint2D32f* cross) {
	double ex1, ey1, ex2, ey2;
	double px1, py1, px2, py2;
	double del;
	double delA, delB, delX, delY;
	double alpha, betta;

	ex1 = p1_start.x;
	ey1 = p1_start.y;
	ex2 = p1_end.x;
	ey2 = p1_end.y;

	px1 = v2_start.x;
	py1 = v2_start.y;
	px2 = v2_end.x;
	py2 = v2_end.y;

	del = (ex1 - ex2) * (py2 - py1) + (ey2 - ey1) * (px2 - px1);
	if ( del == 0) {
		return -1;
	}

	delA =  (px1 - ex1) * (py1 - py2) + (ey1 - py1) * (px1 - px2);
	delB =  (ex1 - px1) * (ey1 - ey2) + (py1 - ey1) * (ex1 - ex2);

	alpha =  delA / del;
	betta = -delB / del;

	if ( alpha < 0 || alpha > 1.0 ) {
		return -1;
	}

	delX =  (ex1 - ex2) * (py1 * (px1 - px2) - px1 * (py1 - py2)) +
			(px1 - px2) * (ex1 * (ey1 - ey2) - ey1 * (ex1 - ex2));

	delY =  (ey1 - ey2) * (px1 * (py1 - py2) - py1 * (px1 - px2)) +
			(py1 - py2) * (ey1 * (ex1 - ex2) - ex1 * (ey1 - ey2));

	cross->x = (float)( delX / del);
	cross->y = (float)(-delY / del);
	return 1;
}


int icvGetCrossLineDirect(CvPoint2D32f p1, CvPoint2D32f p2, float a, float b, float c, CvPoint2D32f* cross) {
	double del;
	double delX, delY, delA;

	double px1, px2, py1, py2;
	double X, Y, alpha;

	px1 = p1.x;
	py1 = p1.y;

	px2 = p2.x;
	py2 = p2.y;

	del = a * (px2 - px1) + b * (py2 - py1);
	if ( del == 0 ) {
		return -1;
	}

	delA = - c - a * px1 - b * py1;
	alpha = delA / del;

	if ( alpha < 0 || alpha > 1.0 ) {
		return -1;/* no cross */
	}

	delX = b * (py1 * (px1 - px2) - px1 * (py1 - py2)) + c * (px1 - px2);
	delY = a * (px1 * (py1 - py2) - py1 * (px1 - px2)) + c * (py1 - py2);

	X = delX / del;
	Y = delY / del;

	cross->x = (float)X;
	cross->y = (float)Y;

	return 1;
}

int cvComputeEpipoles( CvMatr32f camMatr1,  CvMatr32f camMatr2,
					   CvMatr32f rotMatr1,  CvMatr32f rotMatr2,
					   CvVect32f transVect1, CvVect32f transVect2,
					   CvVect32f epipole1,
					   CvVect32f epipole2) {

	/* Copy matrix */

	CvMat ccamMatr1 = cvMat(3, 3, CV_MAT32F, camMatr1);
	CvMat ccamMatr2 = cvMat(3, 3, CV_MAT32F, camMatr2);
	CvMat crotMatr1 = cvMat(3, 3, CV_MAT32F, rotMatr1);
	CvMat crotMatr2 = cvMat(3, 3, CV_MAT32F, rotMatr2);
	CvMat ctransVect1 = cvMat(3, 1, CV_MAT32F, transVect1);
	CvMat ctransVect2 = cvMat(3, 1, CV_MAT32F, transVect2);
	CvMat cepipole1 = cvMat(3, 1, CV_MAT32F, epipole1);
	CvMat cepipole2 = cvMat(3, 1, CV_MAT32F, epipole2);


	CvMat cmatrP1   = cvMat(3, 3, CV_MAT32F, 0); cvmAlloc(&cmatrP1);
	CvMat cmatrP2   = cvMat(3, 3, CV_MAT32F, 0); cvmAlloc(&cmatrP2);
	CvMat cvectp1   = cvMat(3, 1, CV_MAT32F, 0); cvmAlloc(&cvectp1);
	CvMat cvectp2   = cvMat(3, 1, CV_MAT32F, 0); cvmAlloc(&cvectp2);
	CvMat ctmpF1    = cvMat(3, 1, CV_MAT32F, 0); cvmAlloc(&ctmpF1);
	CvMat ctmpM1    = cvMat(3, 3, CV_MAT32F, 0); cvmAlloc(&ctmpM1);
	CvMat ctmpM2    = cvMat(3, 3, CV_MAT32F, 0); cvmAlloc(&ctmpM2);
	CvMat cinvP1    = cvMat(3, 3, CV_MAT32F, 0); cvmAlloc(&cinvP1);
	CvMat cinvP2    = cvMat(3, 3, CV_MAT32F, 0); cvmAlloc(&cinvP2);
	CvMat ctmpMatr  = cvMat(3, 3, CV_MAT32F, 0); cvmAlloc(&ctmpMatr);
	CvMat ctmpVect1 = cvMat(3, 1, CV_MAT32F, 0); cvmAlloc(&ctmpVect1);
	CvMat ctmpVect2 = cvMat(3, 1, CV_MAT32F, 0); cvmAlloc(&ctmpVect2);
	CvMat cmatrF1   = cvMat(3, 3, CV_MAT32F, 0); cvmAlloc(&cmatrF1);
	CvMat ctmpF     = cvMat(3, 3, CV_MAT32F, 0); cvmAlloc(&ctmpF);
	CvMat ctmpE1    = cvMat(3, 1, CV_MAT32F, 0); cvmAlloc(&ctmpE1);
	CvMat ctmpE2    = cvMat(3, 1, CV_MAT32F, 0); cvmAlloc(&ctmpE2);

	/* Compute first */
	cvmMul( &ccamMatr1, &crotMatr1, &cmatrP1);
	cvmInvert( &cmatrP1, &cinvP1 );
	cvmMul( &ccamMatr1, &ctransVect1, &cvectp1 );

	/* Compute second */
	cvmMul( &ccamMatr2, &crotMatr2, &cmatrP2 );
	cvmInvert( &cmatrP2, &cinvP2 );
	cvmMul( &ccamMatr2, &ctransVect2, &cvectp2 );

	cvmMul( &cmatrP1, &cinvP2, &ctmpM1);
	cvmMul( &ctmpM1, &cvectp2, &ctmpVect1);
	cvmSub( &cvectp1, &ctmpVect1, &ctmpE1);

	cvmMul( &cmatrP2, &cinvP1, &ctmpM2);
	cvmMul( &ctmpM2, &cvectp1, &ctmpVect2);
	cvmSub( &cvectp2, &ctmpVect2, &ctmpE2);


	/* Need scale */

	cvmScale(&ctmpE1, &cepipole1, 1.0);
	cvmScale(&ctmpE2, &cepipole2, 1.0);

	cvmFree(&cmatrP1);
	cvmFree(&cmatrP1);
	cvmFree(&cvectp1);
	cvmFree(&cvectp2);
	cvmFree(&ctmpF1);
	cvmFree(&ctmpM1);
	cvmFree(&ctmpM2);
	cvmFree(&cinvP1);
	cvmFree(&cinvP2);
	cvmFree(&ctmpMatr);
	cvmFree(&ctmpVect1);
	cvmFree(&ctmpVect2);
	cvmFree(&cmatrF1);
	cvmFree(&ctmpF);
	cvmFree(&ctmpE1);
	cvmFree(&ctmpE2);

	return CV_NO_ERR;
}/* cvComputeEpipoles */


/* Compute epipoles for fundamental matrix */
int cvComputeEpipolesFromFundMatrix(CvMatr32f fundMatr,
									CvPoint3D32f* epipole1,
									CvPoint3D32f* epipole2) {
	/* Decompose fundamental matrix using SVD ( A = U W V') */
	CvMat fundMatrC = cvMat(3, 3, CV_MAT32F, fundMatr);

	CvMat* matrW = cvCreateMat(3, 3, CV_MAT32F);
	CvMat* matrU = cvCreateMat(3, 3, CV_MAT32F);
	CvMat* matrV = cvCreateMat(3, 3, CV_MAT32F);

	/* From svd we need just last vector of U and V or last row from U' and V' */
	/* We get transposed matrixes U and V */
	cvSVD(&fundMatrC, matrW, matrU, matrV, CV_SVD_V_T | CV_SVD_U_T);

	/* Get last row from U' and compute epipole1 */
	epipole1->x = matrU->data.fl[6];
	epipole1->y = matrU->data.fl[7];
	epipole1->z = matrU->data.fl[8];

	/* Get last row from V' and compute epipole2 */
	epipole2->x = matrV->data.fl[6];
	epipole2->y = matrV->data.fl[7];
	epipole2->z = matrV->data.fl[8];

	cvReleaseMat(&matrW);
	cvReleaseMat(&matrU);
	cvReleaseMat(&matrV);
	return CV_OK;
}

int cvConvertEssential2Fundamental( CvMatr32f essMatr,
									CvMatr32f fundMatr,
									CvMatr32f cameraMatr1,
									CvMatr32f cameraMatr2) {
	/* Fund = inv(CM1') * Ess * inv(CM2) */

	CvMat essMatrC     = cvMat(3, 3, CV_MAT32F, essMatr);
	CvMat fundMatrC    = cvMat(3, 3, CV_MAT32F, fundMatr);
	CvMat cameraMatr1C = cvMat(3, 3, CV_MAT32F, cameraMatr1);
	CvMat cameraMatr2C = cvMat(3, 3, CV_MAT32F, cameraMatr2);

	CvMat* invCM2  = cvCreateMat(3, 3, CV_MAT32F);
	CvMat* tmpMatr = cvCreateMat(3, 3, CV_MAT32F);
	CvMat* invCM1T = cvCreateMat(3, 3, CV_MAT32F);

	cvTranspose(&cameraMatr1C, tmpMatr);
	cvInvert(tmpMatr, invCM1T);
	cvmMul(invCM1T, &essMatrC, tmpMatr);
	cvInvert(&cameraMatr2C, invCM2);
	cvmMul(tmpMatr, invCM2, &fundMatrC);

	/* Scale fundamental matrix */
	double scale;
	scale = 1.0 / fundMatrC.data.fl[8];
	cvConvertScale(&fundMatrC, &fundMatrC, scale);

	cvReleaseMat(&invCM2);
	cvReleaseMat(&tmpMatr);
	cvReleaseMat(&invCM1T);

	return CV_OK;
}

/* Compute essential matrix */

int cvComputeEssentialMatrix(  CvMatr32f rotMatr,
							   CvMatr32f transVect,
							   CvMatr32f essMatr) {
	float transMatr[9];

	/* Make antisymmetric matrix from transpose vector */
	transMatr[0] =   0;
	transMatr[1] = - transVect[2];
	transMatr[2] =   transVect[1];

	transMatr[3] =   transVect[2];
	transMatr[4] =   0;
	transMatr[5] = - transVect[0];

	transMatr[6] = - transVect[1];
	transMatr[7] =   transVect[0];
	transMatr[8] =   0;

	icvMulMatrix_32f(transMatr, 3, 3, rotMatr, 3, 3, essMatr);

	return CV_OK;
}


